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Abstract 

Post  flight  analysis  of  ballistic  missile  reentry  vehicles  is  an  area  of  focus  for  the 
U.S.  Government,  especially  for  those  involved  in  ballistic  missile  defense.  Typically, 
this  analysis  incorporates  either  a  model-driven  least  squares  filter  or  a  data-following 
Kalman  filter.  The  research  performed  here  developed  a  filter  that  attempts  to  integrate 
the  strengths  of  both  filters.  A  least  squares  filter  operates  on  observation  data  collected 
during  exoatmo spheric  free  flight  and  a  Kalman  filter  is  used  to  analyze  data  collected 
lower  in  the  atmosphere,  where  potential  maneuvers  could  be  performed.  Additionally, 
the  filter  was  written  to  incorporate  data  from  multiple  sensors. 

Using  this  hybrid  filter,  different  scenarios  are  investigated  to  determine  the 
potential  benefits  of  adding  additional  collectors,  increasing  the  data  rate  of  collecting 
sensors,  and  investigating  the  effects  of  different  collector  geometry  on  the  accuracy  of 
results. 

Results  show  that  the  filter  successfully  transitions  from  the  least  squares  to 
Kalman  filter,  using  the  final  values  of  the  free  flight  propagation  for  the  Kalman  filter’s 
initial  state.  Using  this  filter  to  investigate  different  collection  scenarios,  it  was 
determined  that  the  best  results  are  achieved  when  multiple  collectors  are  used,  the  data 
collection  rate  of  the  collectors  is  increased,  and  collectors  are  positioned  perpendicular 
to  the  reentry  vehicle  heading. 
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ESTIMATING  CHARACTERISTICS  OF  A  MANEUVERING  REENTRY 
VEHICLE  OBSERVED  BY  MULTIPLE  SENSORS 


I.  Introduction 


Motivation 

Ballistic  missiles  are  among  the  most  advanced  technology  being  currently 
developed  for  the  purpose  of  conducting  war.  From  the  relatively  small  scale  missiles  in 
the  arsenals  of  India  and  Pakistan  to  the  massive  intercontinental  ballistic  missiles 
(ICBMs)  whose  silos  and  mobile  launchers  dot  the  remote  landscapes  of  Russia  and  the 
United  States,  ballistic  missile  technology  exists  today  on  a  massive  scale  and  will  be  a 
major  component  in  future  conflicts.  Additionally,  the  proliferation  of  missile 
technology  to  countries  that  lack  the  technical  manufacturing  expertise  necessary  to 
produce  ballistic  missiles  themselves  is  a  reality.  For  these  reasons,  the  understanding  of 
the  operational  capabilities  of  ballistic  missiles  and  their  reentry  vehicles  (RVs)  is 
currently  one  of  our  nation’s  highest  priorities  [14]. 

To  meet  these  priorities,  the  United  States  has  deployed  a  wide  range  of  sensor 
technologies  throughout  the  world.  Combining  the  data  collected  by  these  sensors  into  a 
coherent  assessment  of  a  missile  system’s  capabilities  has  long  been  the  mission  of 
intelligence  agencies.  Almost  always,  this  will  require  data  analysis  in  the  form  of 
modeling  and  simulation  to  determine  key  characteristics  of  the  missile’s  RV. 
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Background 


The  RV  parameter  most  commonly  estimated  is  the  ballistic  coefficient.  The 
ballistic  coefficient  can  have  varying  definitions  but  is  generally  a  ratio  between  the  RV’s 

mass  and  the  product  of  its  coefficient  of  drag  and  wetted  surface  area  of  the  form  [9] 

m 


P  = 


CDS 


(1) 


where 


[>  =  ballistic  coefficient 


m  =  mass 

Cd  =  coefficient  of  drag 
S  =  wetted  surface  area 


In  this  form,  an  RV-like  object  with  a  large  mass  to  surface  area  ratio  will  be 
referred  to  as  a  high-beta  object  (5000  <  /?  <  15,000  ^);  however,  some  formulations 
can  define  the  term  to  be  the  inverse  of  this  representation  leading  to  RV-like  objects 
being  characterized  by  ballistic  coefficients  that  are  positive  fractions  much  less  than  one. 
In  the  intelligence  community,  the  formulation  with  mass  in  the  numerator,  as  in 
Equation  (1),  is  most  commonly  used  and  will  be  adopted  for  this  thesis. 

Research  Focus  and  Problem  Statement 

While  mass  and  wetted  area  can  largely  be  considered  constant  for  an  RV  flying  a 
purely  ballistic  trajectory,  the  coefficient  of  drag  cannot  be.  Changes  in  velocity  and 
atmospheric  density  will  lead  to  variations  in  the  coefficient  of  drag,  and  thus,  the 
ballistic  coefficient.  Estimating  the  characteristic  profile  of  the  ballistic  coefficient  as  it 
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changes  throughout  the  reentry  for  non- maneuvering  RVs  is  one  of  the  goals  of  the  data 
analysis. 

For  maneuvering  RVs,  the  ability  to  determine  the  accelerations  that  deviate  from 
a  purely  ballistic  reentry  becomes  the  focus  of  analysis.  This  thesis  will  investigate 
methods  of  characterizing  the  magnitude  and  direction  of  these  sensed  accelerations. 

A  further  challenge  in  the  estimation  problem  is  combining  observation  data  from 
multiple  sensors  that  collect  data  on  the  same  target.  The  analytical  problem  that  is  most 
commonly  investigated  involves  the  exploitation  of  single-source  radar  collections  of 
azimuth,  elevation,  range,  and  potentially  range  rate.  As  infrared  sensors,  or  other 
sensors  that  lack  the  ability  to  determine  range,  increasingly  monitor  reentries, 
incorporating  data  from  this  sensor  type  into  the  estimation  problem  is  an  additional  goal 
of  this  thesis.  In  summary,  the  goal  of  this  thesis  is: 

Estimate  characteristics  of  reentry  vehicles,  including  the  bcdlistic  coefficient  and 
non- gravitational  accelerations,  using  obserx’ation  data  collected  by  multiple 
sensors. 

Methodology 

Data  analyses  of  reentering  objects  typically  employ  either  the  method  of  least 
squares  [16  7]  or  a  Kalman  filter  [3,8,9].  The  decision  to  use  one  of  these  filters  over  the 
other  is  usually  determined  by  the  ability  to  model  the  vehicle’s  dynamics  in  a 
predetermined  model.  In  a  situation  where  a  vehicle  is  only  acted  on  by  the  forces  of 
gravity  or  by  constant  sensed  accelerations,  a  least  squares  filter  with  a  model  of  these 
dynamics  will  rapidly  determine  the  best  solution  [1,6].  Alternatively,  a  target 
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experiencing  unpredictable  accelerations  that  could  vary  throughout  its  flight  is  better 
filtered  by  a  Kalman  filter  which  can  adapt  to  changing  dynamics  [3’8’91. 

In  this  thesis,  an  attempt  will  be  made  to  merge  these  two  filters  into  a  single 
algorithm.  During  periods  of  purely  ballistic  flight,  the  accurate  and  fast  solution  of  the 
least  squares  filter  will  be  utilized.  When  the  target  has  descended  lower  into  the 
atmosphere,  where  there  is  a  potential  for  maneuvering,  the  adaptable  Kalman  filter  will 
be  incorporated  to  estimate  non-gravitational  accelerations.  Through  the  marriage  of 
these  two  methods,  the  strengths  of  both  filters  will  be  exercised. 

Assumptions/Limitations 

The  preliminary  implementation  of  this  algorithm  will  assume  separate  zones 
between  the  regions  of  purely  ballistic  flight  and  potential  maneuverability  at  a 
predetermined  altitude.  However,  this  assumption  could  be  invalidated  by  the  firing  of 
thrusters  or  an  upper  stage  rocket  engine  that  could  be  included  in  the  collected  data,  and 
thus  a  future  version  of  this  algorithm  could  allow  for  a  more  detailed  breakdown  of 
regions  of  ballistic  and  maneuvering  flight  to  compensate  for  this.  Potentially,  allowing 
the  Kalman  filtering  portion  of  the  filter  to  activate  during  these  regions  could  handle  any 
such  maneuvers. 

Alternatively,  there  could  be  situations  where  a  vehicle’s  performance  in  the 
lower  atmosphere  is  well  known  and  could  be  modeled.  By  employing  the  Kalman  filter 
during  these  segments  instead  of  a  model  driven  least  squares  filter,  some  accuracy  could 
potentially  be  sacrificed.  This  circumstance  is  unlikely  due  to  the  unpredictable  nature  of 
reentry  vehicles  and  would  be  unlikely  to  arise. 


4 


For  all  cases  investigated,  sensors  measuring  range,  azimuth,  and  elevation  (RAE) 
or  azimuth  and  elevation  (AE)  were  incorporated.  Random  noise  was  applied  to 
simulated  observation  data  according  to  Table  1.  These  values  were  specified  in  an 
attempt  to  be  representative  of  these  categories  of  sensors  without  being  specific  to  any 
actual  sensor  in  the  real  world.  Having  a  priori  knowledge  of  these  values  is  assumed. 


Table  1.  Random  noise  applied  to  simulated  observations. 


Standard  Deviation 

RAE  Sensor 

AE  Sensor 

Grange 

2  m 

— 

° AZIMUTH 

.02  degrees 

.03  degrees 

°ELEVATlON 

.015  degrees 

.03  degrees 

Preview 

In  the  following  pages,  Chapter  II  will  review  the  published  literature  covering 
previous  research  on  this  topic,  Chapter  III  will  provide  a  detailed  methodology  for  the 
data  filter’s  operation,  Chapter  IV  discusses  the  results  of  this  methodology,  and  Chapter 
V  provides  conclusions  and  offers  recommendations  for  future  work.  After  the  main 
body  of  the  paper,  the  appendix  includes  MATLAB  code  written  as  a  part  of  this 
research. 
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II.  Literature  Review 


Chapter  Overview 

The  purpose  of  this  chapter  is  to  provide  an  overview  of  relevant  research  that  has 
been  conducted  in  the  past  ten  years  pertaining  to  this  topic. 

Relevant  Research 

The  research  into  nonlinear  estimation  has  a  long  history,  arguably  dating  back 
well  before  the  time  of  Gauss’s  development  of  least  squares  to  the  methods  of 
averaging.  The  advances  in  the  field  have  largely  mirrored  the  advances  in  computing 
technology,  as  described  in  Nonlinear  Filters:  Beyond  the  Kalman  Filter  [3].  Gauss 
developed  a  method  to  make  a  single  pass  through  all  the  available  observations  at  once. 
With  the  advances  in  the  computing  technology,  engineers  increasingly  turned  to 
sequential  filters,  such  as  the  unscented  Kalman  filter,  to  perform  real-time  estimation. 
As  advances  in  computing  progress,  even  more  exotic  methods  have  begun  to  gain  in 
popularity,  such  as  the  particle  filter.  The  particle  filter  is  based  off  of  Monte  Carlo 
sampling  being  used  to  investigate  the  state  space  [3,15]. 

Some  research  has  been  performed  at  the  Air  Force  Institute  of  Technology 
(AFIT)  by  Holmes  [6]  and  Bittle  [1]  into  the  parameter  identification  of  reentry  vehicles. 
The  research  of  Holmes  and  Bittle  primarily  focused  on  identifying  characteristics  of 
reentry  vehicles  that  were  in  either  flying  purely  ballistic  or  performing  a  constant 
maneuver  which  could  be  determined  from  the  vehicle’s  bank  angle  [1,6].  Both  of  these 
problems  lend  themselves  to  the  batch  processing  of  the  method  of  least  squares,  but 
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would  not  be  able  to  handle  the  issue  of  a  ballistic  coefficient  changing  as  a  function  of 
velocity  and  altitude.  In  order  to  account  for  these  uncertain  dynamics,  this  thesis  will 
move  beyond  the  batch  processing  of  the  method  of  least  squares  into  a  sequential  filter, 
like  the  Kalman  filter.  As  will  be  shown,  to  maintain  the  maximum  accuracy  the 
solutions  of  both  a  Kalman  filter  and  a  nonlinear  least  squares  filter  will  be  combined  into 
a  hybrid  solution. 

Some  work  that  has  been  done  on  hybrid  filters,  or  modified  filters  [7,9]  mostly 
focused  on  the  problem  of  RV  interception  by  an  Anti-Ballistic  Missile  (ABM).  Jackson 
and  Farbman  developed  an  interesting  application  of  the  least  squares  method  where  data 
is  processed  in  small  batches  instead  of  as  one  large  data  set.  This  method  allowed  the 
filter  to  respond  to  changing  dynamics.  Jackson  and  Farbman’s  approach  could  be  of 
applicability  in  this  problem,  but  they  relied  heavily  on  curve  fitting  data  without  regard 
to  continuity  of  dynamics  between  adjacent  states.  Also,  accelerations  were  modeled  as 
unknowns  in  three  directions,  so  further  processing  would  need  to  take  place  to  determine 
the  sources  of  accelerations  and  what  portion  of  the  entire  acceleration  was  due  to  drag. 

Lee  and  Liu  adopted  a  more  hybrid  approach,  combining  a  least  squares  and 
Kalman  filters  [9].  Lee  and  Liu  recognized  that  a  Kalman  filter  is  better  suited  to  a  target 
with  large  changes  in  dynamics,  but  that  when  such  a  filter  is  applied  to  better  behaved 
vehicles  (slowly  changing  EOMs)  results  can  degrade  greatly.  To  counter  this  behavior, 
Lee  and  Liu  ran  a  Kalman  filter  with  a  basic  state  vector  through  portions  of  the  flight 
identified  as  being  nearly  ballistic  by  the  companion  least  squares  filter  and  then  switched 
modes  to  a  more  dynamic  state  vector  for  the  Kalman  filter  when  the  companion  least 
squares  filter  identifies  potential  non-ballistic  behavior.  This  thesis  will  attempt  to  utilize 
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the  methods  of  Jackson  and  Farbman  to  estimate  an  initial  guess  for  the  state  and  further 
refine  this  estimate  using  a  Kalman  filter  similar  to  that  derived  by  Lee  and  Liu. 

Summary 

A  lot  of  research  is  being  done  on  new  methods  of  data  filtering,  including 
particle  filters  and  other  exotic  filtering  techniques.  This  thesis  will  investigate  a 
combination  of  the  more  classical  least  squares  and  Kalman  filters  to  take  advantage  of 
their  individual  strengths.  This  method  is  different  than  what  was  developed  by  Lee  and 
Liu,  where  the  companion  least  squares  filter  was  only  used  to  modify  the  makeup  of  the 
Kalman  filter’s  state  vector.  A  method  of  performing  least  squares  filtering  on  a  sliding 
window  of  non-ballistic  flight  will  be  explored  to  provide  an  initial  guess  at  the  state  for 
the  Kalman  filter  to  include  in  its  computations.  This  least  squares  sliding  window 
method  was  pulled  from  the  literature,  where  it  was  used  to  generate  the  final  estimate, 
rather  than  acting  as  preprocessing  for  another  filter.  Furthermore,  whereas  all  of  the 
reviewed  literature  assumed  a  single  collector  and  sometimes  simplified  observation 
models,  the  algorithm  developed  here  accommodates  an  unlimited  number  of  sensors  and 
a  variety  of  data  types. 


8 


III.  Methodology 


Chapter  Overview 

The  purpose  of  this  chapter  is  to  overview  the  specific  methods  and  decisions  that 
were  implemented  for  the  filters  included  in  this  thesis.  A  breakdown  of  the  different 
filtering  phases  as  well  as  assumptions  and  engineering  decisions  will  be  described. 

Filter  Phases 

Phase  0  -  The  Initial  Guess. 

The  phase  of  flight  where  the  target  is  exo atmospheric  is  analyzed  by  a  batch  least 
squares  filter  fitting  the  observations  to  an  oblate  Earth  gravity  model.  This  phase  is 
implemented  for  all  observations  that  are  determined  to  be  at  an  altitude  greater  than  120 

:fc 

km.  The  120  km  value  is  used  as  a  common  number  with  METAL  for  consistency.  If 
the  data  does  not  include  any  observations  above  this  cutoff  altitude,  the  algorithm  will 
instead  consider  the  highest  altitude  observations.  In  this  case,  the  algorithm  will  select 
10%  of  the  entire  data  set  that  occurs  at  the  highest  altitudes  to  fit  a  state  vector  to. 

With  the  free  flight  data  identified,  the  initial  hurdle  for  the  least  squares  filter  is 
determining  an  initial  guess  to  feed  the  batch  filter.  For  this  thesis,  an  initial  state  was 
computed  using  Equation  (1)  [7],  the  same  equation  that  will  be  used  for  the  least  squares 
sliding  window.  For  this  application,  this  portion  of  data  is  taken  as  the  window  of 
interest  and  a  single  computation  is  done.  The  equation  describing  the  state’s 


The  Mathematical  and  Engineering  Trajectory  Analysis  Library  is  a  library  of  mostly  MATLAB  functions 
that  was  developed  at  the  National  Air  and  Space  Intelligence  Center  that  contains  tools  to  accomplish 
common  trajectory  analysis  tasks,  such  as  coordinate  frame  conversions,  state  vector  propagation,  and  data 
manipulation.  121 
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propagation  with  constant  snap  (fourth-derivative  of  position)  is  presented  in  Equation 

(2)  [7]. 


=  On)  +  (*n(1))At  +  (xn(2)UA  t2  +  (x,/3))Ot3  +  (xn(4))OA  tA 


where 


(x  )meas  =  obserx’ed  positions 

(xn)  =  initial  state  to  be  computed 

xn^  =  ith  inertial  derivative  of  the  state  with  respect  to  time 
At  =  time  difference  between  observations  and  initial  state 


Equation  (2)  can  be  used  to  separately  solve  for  the  X,  Y,  and  Z  components  of 
position  and  velocity  for  the  initial  state  estimate.  The  procedure  for  computing  the 
initial  X  position  and  velocity  is  now  described,  with  the  understanding  that  the 
procedure  is  the  same  for  the  Y  and  Z  components. 

In  order  to  compute  the  position  and  velocity  components  of  the  state,  a  system  of 
equations  of  the  form  Ax  =  b  is  desired  so  that  the  least  squares  solution  can  be  directly 
solved  for.  In  this  application,  the  A  matrix  has  as  many  rows  as  observation  points  and 

five  columns  matching  the  five  states  defined  in  Equation  (2).  The  A  matrix  defines  the 

coefficients  of  the  derivatives  defined  by  Equation  (2)  as  shown  below: 

1  2  I  3  I  4- 

1  Ati  -Ati  -Ati  —Ati 

1  2  1  6  1  24  1 

1  2  1  3  1  4  (3) 

1  A  t2  -At  2  -A  t2  —At  2  f 

z  2  /  6  A  24  z 
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The  b  vector  is  constructed  as  the  X  position  components  of  observations  that  had 


been  previously  identified  as  being  exoatmospheric  transformed  to  an  ECI  reference 
frame.  The  b  vector  has  as  many  rows  as  observations  and  is  a  single  column.  With  this 
information  defined,  the  system  of  equations  is  fully  defined  as: 


1  Ati  -AT 
1  2  1 


2  I  3  I  A 

&  ^ 


1  At  2  -At,  -At,  —At, 

z  2  /  6  z  24  z 


r  xn  ] 

X  W 

A,n 

xmeas  \ 

X  W 

A,n 

= 

xmeas  2 

x  (3> 

A,n 

: 

£ 

* 

(4) 


The  ideal  solution  to  this  system,  when  A  is  square  and  nonsingular,  is  x  —  A_1b 
however  this  thesis  will  take  advantage  of  the  Singular  Value  Decomposition  (SVD)  to 
perform  the  inversion,  which  will  have  the  advantage  of  computing  the  minimum  norm 
solution  when  the  A  matrix  is  not  invertible  [1I].  This  calculation  is  done  easily  with 
MATLAB  using  the  pinv  ( )  command  to  compute  the  pseudo  inverse  of  A.  Since  the 
pseudoinverse  equals  the  inverse  in  the  case  where  A  is  invertible,  this  method  is 
appropriate  for  all  A. 

Once  this  procedure  has  been  performed  on  the  X,  Y,  and  Z  elements  of  the 
transformed  observations,  the  initial  guess  of  the  state  is  taken  as  the  position  and 
velocity  components  of  the  individual  solutions.  With  this  initial  guess,  the  batch  least 
squares  filter  is  triggered. 

Phase  1  -  Free  Flight  Batch  Least  Squares  Filter. 

The  batch  least  squares  filter  operates  iteratively,  improving  upon  the  solution 
until  further  computation  cannot  achieve  a  better  result.  The  least  squares  filter  relies  on 
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the  computation  of  partial  derivatives  to  compute  state  updates.  In  the  classical 
implementation  of  the  filter,  these  derivatives  are  derived  analytically  [13],  but  with 
today’s  computers  these  derivatives  can  easily  be  estimated  numerically  using  a  finite 
difference  method.  This  thesis  will  implement  numerical  partial  derivatives  for 
computing  the  state  updates  in  the  same  manner  as  does  portions  of  METAF[2].  With  this 
decision,  the  least  squares  algorithm  will  proceed  as  described  in  Figure  1  [13]. 


Figure  1  -  Nonlinear  least  squares  flowchart 


In  MATFAB,  the  current  guess  at  the  initial  state  is  taken  as  xrej  (t0).  The 
elements  of  the  state  used  throughout  the  algorithm  are: 


12 


'  XECI  ' 

Yeci 

ZECI 

XECI 

Yeci 

ZECI 

*Seci 

ysEci 

-^SeCI- 


(5) 


where 


XECI 

ysa 

ZECI 

XECI 

Yeci 

ZECI 


XS 


ECI 


ys 


ECI 


=  Earth-Centered  Inertial  X  Position 
=  Earth-Centered  Inertial  Y  Position 
=  Earth-Centered  Inertial  Z  Position 
=  Earth-Centered  Inertial  X  Velocity 
=  Earth-Centered  Inertial  Y  Velocity 
=  Earth-Centered  Inertial  Z  Velocity 
=  Earth-Centered  Inertial  X  Sensed  Acceleration 
=  Earth-Centered  Inertial  Y  Sensed  Acceleration 


zs eci  =  Earth- Centered  Inertial  Z  Sensed  Acceleration 


Note  that  the  accelerations  in  Equation  (5)  are  sensed  accelerations.  Sensed 
accelerations  differ  from  the  total  accelerations  in  that  they  do  not  incorporate 
gravitational  acceleration.  Sensed  accelerations  account  for  accelerations  that  are  the 
result  of  other  body  forces  that  would  result  in  a  maneuver  away  from  a  ballistic 
trajectory. 
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This  state  is  then  perturbed  individually  in  the  X,  Y,  and  Z  positions  and 
velocities  to  create  six  perturbed  states  denoted  xpert(t0).  The  magnitudes  of  these 
perturbations  are  arbitrarily  chosen.  For  this  thesis,  perturbations  of  1  m  in  position  and 
1  ^  in  velocity  are  implemented  as  arbitrary  values. 

With  the  initial  state  and  perturbed  states  defined,  they  are  then  propagated  to  all 
observation  times.  This  is  accomplished  by  basic  equations  of  motion  (EOM)  defined  in 
Equations  (6-11). 


(6) 


d 

dt 


y  =  y 


(7) 


(8) 


d  . 

—x  —  x  —  qY 

dt  t,x 


^y  =  y  =  9y 


d  . 

—z  =  z  =  q7 

dt  az 


(9) 

(10) 
(11) 


where 


dx’dydz  =  x,y,and  z  ECI  components  of  gravitational  acceleration 


During  this  phase  of  flight,  it  is  assumed  that  there  is  little  to  no  atmosphere  and 
therefore  there  are  no  external  body  forces  and  hence  no  sensed  accelerations.  The  only 
changes  in  velocity  are  due  to  the  gravitational  acceleration.  With  the  current  state 
estimate  and  its  perturbations  propagated,  the  next  step  is  the  computation  of  the  data 
residuals. 
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Computing  data  residuals  is  merely  an  exercise  in  reference  frame 
transformations.  These  functions  have  been  fully  implemented  in  the  METAL  library, 
and  were  used  in  this  research  [2].  The  propagated  states  are  transformed  from  Earth- 
Centered  Inertial  (ECI)  frame  to  a  sensor-specific  Range,  Azimuth,  and  Elevation  (RAE) 
frame.  With  this  transformation  complete,  the  residuals  are  calculated  from  Equation 
(12). 


f  =  £  -  *<  ef  RM  (12) 

where 

r  =  matrix  of  residuals 
z  =  matrix  of  observations 
xref  rae  =  ProPa8ated  reference  state  in  RAE  frame 

With  the  residuals  computed,  statistical  editing  of  outliers  can  be  performed. 
With  a  priori  knowledge  of  a  sensor’s  performance^,  residuals  outside  of  an  arbitrary 
number  of  standard  deviations  can  be  removed.  The  algorithm  produced  for  this  thesis 
allows  for  the  number  of  standard  deviations  to  be  input  by  the  user,  with  the  default 
settings  deleting  residuals  more  than  three  standard  deviations  from  the  computed 
reference  trajectory. 

With  any  deleting  complete,  the  residuals  matrix  must  be  reshaped  into  a  column 
vector  to  be  used  in  the  sum  Y,i  T? Qf1^.  Note  that  this  action  is  easily  accomplished  in 
MATLAB  with  the  reshape  ( )  function. 

1  See  Table  1  for  error  values  used  in  the  simulated  observation  data  generated  for  this  research. 
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The  Qi  matrix  required  for  the  two  sums  is  constructed  from  the  a  priori 
knowledge  of  random  errors  in  the  sensor  observations.  The  matrix  is  diagonal  and  of  the 
form: 


where 


r  1 

OR2 

0 

0 

0 

o 

0 

1 

OA2 

0 

0 

0  ... 

0 

0 

1 

0£2 

0 

0  ... 

0 

0 

0 

1 

OR2 

0  ... 

1 - - 

..  O 

0 

0 

0 

1 

OA2 

(13) 


Q  =  covariance  matrix 

aR  =  sensor  standard  deviation  in  range  measurements 
oA  =  sensor  standard  deviation  in  azimuth  measurements 
oE  =  sensor  standard  deviation  in  elevation  measurements 


The  Q  matrix  in  Equation  (13)  has  three  of  the  standard  deviations  repeated 
enough  times  to  make  the  matrix  dimensions  equal  to  the  number  of  observations 
multiplied  by  the  number  of  observation  data  types,  three  in  the  case  of  a  sensor  with 
range,  azimuth,  and  elevation  measurements.  For  sensors  lacking  a  range  component  in 
the  measurement,  the  range  standard  deviation  is  omitted  and  the  Q  matrix  is  reduced  in 
size. 

The  last  element  of  the  sum  7/ needed  is  T,  the  observation  matrix, 
computed  with  the  perturbed  trajectories.  This  matrix  is  defined  as  the  propagation  of  the 
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partials  of  the  observation  relationships  with  respect  to  the  different  state  vector 


components,  as  shown  numerically  in  Equation  (14). 


T  = 

'Eref  ^~Rpert  —x^ 

Rref  1  Rpert  -y  1 

Eref  ^  Epert  — 

Eref  ^  Epert  —x^ 

Eref 

1  Rpert-y1 

Eref 

(14) 

-j ~Epert  —z^ 

Xpert 

ype  rt 

zpert 

Xpert 

ypert 

zpert 

Aref  i  ~ Apert  —x  ^ 

Aref  Apert  — 

Apef  Apert  — 

Aref  Apert  —x^ 

Aref 

1  ~Apert  -y  1 

Aref 

Apert  — 

Xpert 

ypert 

zpert 

Xpert 

ypert 

zpert 

b ref  ^~bpert  —x^ 

^ref  ^  ~Epert  —y  ^ 

Eref  i  ~Epert  —z  ^ 

Eref  ^  ~Epert  —x  ^ 

Eref 

1  ~Epert  -y  1 

Eref 

Epert  —z^ 

Xpert 

ypert 

zpert 

Xpert 

Vpert 

zpert 

where 


T 

R ref  i 
A ref  i 

Eref  1 
Epert  —x^ 

A 

npert  —x^ 

p 

upert  —x-^ 
Xpert 


=  observation  matrix 

=  first  range  value  of  the  propagated  reference  trajectory 
=  first  azimuth  value  of  the  propagated  reference  trajectory 
=  first  elevation  value  of  the  propagated  reference  trajectory 
=  first  range  value  of  the  x-perturbed  trajectory 
=  first  azimuth  value  of  the  x-perturbed  trajectory 
=  first  elevation  value  of  the  x-perturbed  trajectory 
=  magnitude  of  perturbation  to  reference  state  in  x  direction 


With  all  elements  of  the  sums  T-'Qj1  TL  and  X,  T-r  Qj1  r,  now  computed,  these 
values  are  now  incorporated  into  the  sums  and  the  updates  are  computed.  From  Figure 
1,  these  updates  are 
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(15) 


where 


Sx(to)  =  PSx 


(16) 


Psx  =  updated  covariance  matrix 
Sx(t0)  =  update  to  the  reference  state 


The  final  step  in  the  batch  least  squares  phase  is  to  check  for  convergence  and 
update  the  reference  state.  The  test  for  convergence  can  be  accomplished  in  a  couple  of 
ways,  either  by  checking  if  the  update  to  the  state  vector  lies  within  the  updated 
covariance  matrix  uncertainty  [13]  or  by  checking  to  see  if  a  defined  cost  function  has 
stopped  improving  from  one  iteration  to  the  next  [2].  Both  of  these  methods  were 
investigated,  and  in  the  end  it  was  decided  that  the  cost  function  method  typically  led  to 
better  results  as  the  covariance  method  would  typically  indicate  convergence  before  a 
good  fit  had  been  achieved. 

The  cost  function  computed  for  this  thesis  takes  the  form: 


-  I 

SENSORS 


i OBSERVATIONS 


(■ residuaiy 


aSENSOR 


where 


cost  =  cost  function  used  to  test  for  convergence 
residual  =  obserx’ation  residual  from  reference  trajectory 
°sensor  =  standard  deviation  of  sensor  observation 
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This  cost  function  normalizes  the  data  residuals  by  the  sensor’s  standard 
deviations  and  gives  proper  weight  to  measurements  that  are  more  precise  than  others. 
The  change  in  cost  function  between  iterations  is  computed  in  each  iteration  after  the 
first.  Once  this  change  drops  below  a  value  defined  by  the  user,  the  filter  is  stopped.  The 
default  convergence  criterion  is  a  change  of  less  than  .01%  of  the  cost  function. 

Once  the  free  flight  batch  least  squares  filter  has  converged,  the  final  state  is 
passed  to  the  next  phase  of  the  algorithm,  where  the  sensed  accelerations  of  the  target 
will  be  considered  variable,  and  need  to  be  estimated. 

Phase  2a  -  Least  Squares  Sliding  Window  Discontinuous  Filter. 

The  next  phase  of  the  algorithm  is  divided  into  three  subsections:  2a,  2b,  and  2c, 
which  together  filter  those  observations  deep  enough  into  the  atmosphere  that  the  target 
can  experience  non-zero  sensed  accelerations.  The  first  of  these  subsections  in  the  least 
squares  sliding  window  filter.  This  filter  is  based  off  of  the  paper  by  Jackson  and 
Farbman,  Trajectory  Reconstruction  with  a  Least  Squares  Sliding  Window  (LSSW)  Filter 
[7].  This  subsection  is  a  rapid  computation  of  an  approximate  state  at  every  observation 
that  contains  a  range,  azimuth,  and  elevation  measurement.  This  approximation  can  be 
used  in  the  subsequent  subsections  of  phase  two  as  an  initial  guess  of  the  state  at  those 
times. 

This  section  relies  on  multiple  solutions  to  Equation  (2)  over  different  portions  of 
the  observations.  Taken  as  a  whole,  these  solutions  are  the  initial  guesses  used  later. 
Other  than  computing  solutions  to  Equation  (2)  multiple  times,  this  portion  of  the 
algorithm  must  determine  which  portions  (windows)  of  the  observations  to  include  in 
each  run.  The  method  used  to  determine  this  window  size  is  similar  to  that  performed  by 
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Jackson  and  Farbman.  The  window  size  begins  at  a  defined  minimum  value,  grows  to  a 
nominal  value  that  is  used  throughout  the  majority  of  the  observations,  and  then  grows 
again  as  the  window  approaches  the  end  of  the  available  data.  Three  values  for  minimum 
window  size,  window  size,  and  maximum  window  size  are  optional  user  inputs  with 
default  values  5,  30,  and  40,  respectively.  An  example  of  a  window  size  varying  by  the 

position  in  the  observations  is  presented  in  Figure  2. 
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Figure  2  -  Least  squares  sliding  window  size 

After  some  trial,  it  was  decided  to  limit  the  use  of  these  results  in  later  sections  of  phase 
two  to  just  the  position  estimates.  The  velocity  and  acceleration  estimates  were  found  to 
have  extreme  noise,  often  returning  estimates  with  mean  and  random  errors  large  enough 
that  it  was  unclear  if  there  was  any  correlation  to  the  truth  values.  With  the  positions 
estimates  determined,  the  next  subsection  of  phase  two  is  commenced. 
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Phase  2b  -  Kalman  Filter  Forward  Pass. 


The  main  subsection  of  phase  two  runs  a  Kalman  filter. 


The  Kalman  filter  is 


executed  in  a  manner  described  in  Figure  3  [13]. 


Figure  3  -  The  Kalman  filter  flowchart 

The  state  is  propagated  using  easily  defined  equations  of  motion,  rather  than  the 
state  transition  matrix.  These  equations  of  motion  are 
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(18) 


II 

■«  I’S 

(19) 

d 

—Z  —  Z 
dt 

(20) 

±x  =  x  =  gx  +  % 

(21) 

iy  =  y  =  9y+% 

(22) 

±i  =  z=gz+zs 

(23) 

where 


x,  y,  z  =  total  accelerations  in  the  x,  y,  and  z  ECI  directions 

xs,  ys,  zs  =  sensed  accelerations  ( due  to  body  forces  other  than  gravity) 
in  the  x,  y,  and  z  ECI  directions 

Equations  (18-23)  differ  from  Equations  (6-11)  that  were  used  in  the  free  flight 
batch  least  squares  filter  in  that  the  sensed  accelerations  are  allowed  to  be  non-zero.  This 
method  of  propagating  the  state  is  more  accurate  than  the  approximation  given  by  the 
state  transition  matrix  and  is  implemented  for  state  propagation.  The  state  transition 
matrix  must  still  be  computed,  however,  in  order  to  propagate  the  state  covariance.  The 
state  transition  matrix  is  computed  from 
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where 

=  state  transition  matrix 
F  =  state  distribution  matrix 

At  =  difference  in  time  between  adjacent  observations 


The  state  distribution  matrix,  F,  is  the  Jacobian  of  the  state  dynamics  defined  in 
Equations  (18-23)  where  the  gravitational  acceleration  is  replaced  with  a  simple 
approximation  with  respect  to  the  elements  of  the  state,  defined  in  Equation  (5). 


(25) 

(26) 
(27) 


where 


p  =  Earth ’s  gravitational  parameter  ~  3.986005  ■  10“ 


With  these  values  substituted  for  the  gravitational  acceleration,  the  state 
distribution  matrix  is  computed  by  Equation  (28). 
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(28) 


F  = 


0 

0 

0 

-pR~3(l  -  3 x2R~2) 
3  pxyR~s 
3  pxzR~5 

0 

0 

0 


0 

0 

0 

3  pxyR~s 

pR~3(l  -  3 y2R~2) 
3pyzR~5 
0 
0 
0 


0 

0 

0 

3  pxzR~5 
3pyzR~5 

-pR~3(  1  -  3z2R~2) 
0 
0 
0 


1  0  0  0  0  0 
0  1  0  0  0  0 
0  0  1  0  0  0 
0  0  0  1  0  0 
0  0  0  0  1  0 
0  0  0  0  0  1 
0  0  0  0  0  0 
0  0  0  0  0  0 
0  0  0  0  0  0 


where 


R 


=  V(x2  +  y2  +  z2)  for  ease  of  computation 


With  the  state  distribution  matrix,  F,  computed,  the  state  transition  matrix,  <t>,  can 
be  determined.  The  state  transition  matrix  can  then  be  used  to  propagate  the  covariance 
matrix  to  the  current  time  with  Equation  (29). 

Ptn(-)  =  <DPtn_1(+)<I>T  +  Q  (29) 


where 


V-) 

Q 


=  initial  covariance  matrix  at  time  tn 
=  updated  covariance  matrix  at  time  tn_1 
=  noise  applied  to  covariance  propagation 


A  few  different  methods  of  computing  Q,  the  covariance  propagation  noise,  were 
investigated.  In  the  end,  the  method  employed  by  the  Kinematics  And  Dynamics 
Reconstruction  Environment  (KADRE)[4]  was  implemented  due  to  its  incorporation  of 
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the  time  step.  Due  to  its  explicit  dependency  on  the  time  step  from  one  data  point  to  the 
next,  the  filter  was  better  behaved  when  compared  to  other  methods,  including  a  constant 
Q.  The  computation  of  Q  was  performed  with  Equation  (30). 

n  m 

<?  =  ZZ^F',?"(F,'yAtl+‘'yr hi  <3°) 

i=0  7=0 

where 

F  =  state  distribution  matrix 
Q0  =  covariance  scaling  matrix 
At  =  elapsed  time  from  previous  data  point 


The  scaling  matrix,  Q0,  was  implemented  as  user-defined  variables  that  form  the 

diagonal  matrix  shown  in  Equation  (31). 

qp  00000000- 
0  qp  0000000 
00  qp  000000 
000  qv  00000 
<2o=0  0  0  0  qv  0  0  0  0  (31) 

00000  qv  000 
000000  qa  00 
0000000  qa0 
.00000000  qa. 

where 

qp  =  position  covariance  scaling  constant 
qv  =  velocity  covariance  scaling  constant 
qa  =  acceleration  covariance  scaling  constant 
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The  covariance  scaling  constants  qp,  qv,  and  qa  are  implemented  with  default 
values  of  0,  0,  and  0.001,  respectively.  These  values  were  arbitrarily  selected  after  some 
experimentation. 

With  the  state  and  covariance  propagated  to  the  current  data  point,  the  next  step 
involves  the  computation  of  the  observation  matrix,  H,  which  is  a  linearization  of  the 
relationship  between  the  state  elements  and  the  observation  variables.  This  matrix  can  be 
used  to  convert  both  the  covariance  and  state  variables  to  the  observation  variables 
reference  frame.  As  the  transformation  from  ECI  state  components  to  RAE  observation 
variables  is  easily  handled  with  the  METAL  library,  the  exact  transformation  can  be 
used.  The  observation  matrix  must  still  be  used  to  transform  the  propagated  covariance 
matrix.  The  observation  matrix  is  computed  from  Equations  (32-47). 
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=  sensor  geocentric  latitude 
=  sensor  longitude 
=  Earth  angular  velocity 
=  sensor  ECI  position  at  observation 
=  sensor  ECEF  position 
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This  fully  defines  the  H  matrix  for  range,  azimuth,  and  elevation  observations.  If 
the  solution  from  the  LSSW  subsection  is  included  in  the  Kalman  filter,  those  partial 
derivatives  must  be  included  in  H.  Since  the  LSSW  solution  is  just  the  state  vector 
positions,  those  partials  are  easily  added  as  shown  in  Equation  (48). 
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The  final  matrix  that  must  be  computed  before  determining  the  Kalman  gain,  K  is 
the  measurement  noise  matrix,  R.  For  a  sensor  collecting  range,  azimuth,  and  elevation 
data,  the  R  matrix  is  constructed  by  Equation  (49).  If  the  LSSW  solution  is  also  included, 
its  apparent  noise  is  included  as  shown  in  Equation  (50).  The  initial  implementation  of 
the  algorithm  uses  a  LSSW  error  standard  deviation  that  is  independent  of  direction.  This 


could  be  modified  in  subsequent  work. 
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where 

R  =  measurement  noise  matrix 
oR  =  sensor  range  measurement  standard  deviation 
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oAz  =  sensor  azimuth  measurement  standard  deviation 
aEi  =  sensor  elevation  measurement  standard  deviation 
Gissw  =  LSSW  error  standard  deviation 

With  Ptn  (— ),  H,  and  R.  the  Kalman  gain,  K\  the  updated  covariance,  Ptn  (+);  and 
the  state  update,  5x,  are  computed  from  Equations  (51-53),  respectively. 

K  =  P(-)tfT(fl  +  HP(-)Ht)~1  (51) 

^n(+)  =  (/  —  KH)Ptn  (— )  (52) 

Sx  —  K(z  —  Hx)  (53) 

where 

/  =  identity  matrix 

z  =  vector  of  observations 

x  =  9-element  vector  of  the  current  state 

This  completes  the  Kalman  filter  subsection.  This  series  of  equations  is  carried 
out  at  every  data  point  until  the  end  of  the  data  set.  As  the  computation  takes  place,  a 

time  history  of  the  state  and  covariance  are  saved  for  use  in  the  final  subsection  of  phase 

two,  the  backward  smoother. 

Phase  2c  -  Backward  Smoother  Pass. 

The  final  subsection  of  phase  two  involves  a  backward  traveling  smoother  pass 
using  the  time  history  of  results  from  the  Kalman  filter.  This  section  of  code  was 
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implemented  in  the  manner  described  by  the  KADRE  engineering  description  [4] 
Starting  at  the  final  time,  the  filter  consists  of  Equations  (54-56). 


where 


C  =  Ptri(+)d>7Ptn+1(-)-1  (54) 

Psmoot  htn  =  Ptn  (+)  +  C  ^ Psmoot  htn+1  ~  Ptn+1  (“))  C7  (55) 
% smoot  htn  X{n  +  (xsmoot  htR+1  ^  (56) 

C  =  C  matrix 

xLn+f  (— )  =  pre-update  state  vector  from  Kalman  filter 


Once  the  smoother  pass  is  complete,  the  filtering  phases  of  the  code  are  complete. 
Additional  code  is  included  to  perform  analysis  on  the  results,  including  calculations  of 
altitude,  ballistic  coefficient,  and  Mach  number.  These  calculations  makeup  the  final 
phase  of  the  algorithm. 

Phase  3  -  Wrap-up  and  parameter  computation. 

The  final  phase  of  the  algorithm  performs  calculations  that  can  be  useful  for  the 
analysis  of  results.  With  the  complete  time  history  of  the  state  available,  these 
calculations  are  performed  rapidly  and  included  in  the  output.  Key  among  these 
parameters  for  this  research  is  the  ballistic  coefficient.  With  the  time  history  of  sensed 
accelerations,  the  ballistic  coefficient  is  computed  from  Equations  (57)  and  (58). 
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=  yj (x  +  co  y)2  +  (y  —  a)  x)2  +  z2 


(57) 


where 


Va 

P 


JpVa 


••  2  .»  2  .»  2 
+  ys  +  zs 


(58) 


Va  =  Air  relative  velocity  magnitude 

on  =  Earth ’s  angular  velocity  about  the  pole 

p  =  altitude  dependent  atmospheric  density 


In  addition  to  ballistic  coefficient,  parameters  such  as  altitude,  Mach  number,  and 
the  position  of  the  target  in  latitude  and  longitude  are  computed  for  easier  analysis. 

Summary 

This  chapter  described  the  methodology  used  in  the  algorithm  to  filter  observation 
data.  The  algorithm  operates  in  two  primary  filtering  phases  with  a  third  wrap-up  phase. 
The  first  phase  filters  exoatmospheric  observations  with  a  batch  least  squares  filter  that 
assumes  there  are  no  sensed  accelerations  other  than  gravity.  The  second  phase  filters 
endoatmospheric  observations  with  a  least  squares  sliding  window  filter,  a  Kalman  filter 
forward  pass,  and  a  backward  running  smoother.  The  final,  wrap-up  phase,  calculates 
parameters  that  can  be  useful  for  further  analysis. 
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IV.  Analysis  and  Results 


Chapter  Overview 

This  chapter  reviews  the  analysis  that  was  performed  with  the  filtering  algorithm 
created  following  the  methodology  detailed  in  Chapter  III.  The  primary  goals  of  the 
analysis  performed  were  to  validate  the  filter’s  performance  in  reconstructing  an 
observed  reentering  target,  to  investigate  the  benefits  to  accuracy  of  additional  sensors 
observing  the  same  target,  to  investigate  the  benefits  to  accuracy  of  an  increased  rate  of 
data  collection  against  targets,  and  to  investigate  the  benefits  to  accuracy  of  different 
sensor  collection  geometries.  In  each  case  where  the  accuracy  of  the  filter  is  to  be 
investigated,  the  filter  will  be  tested  against  both  non-maneuvering  and  maneuvering 
targets. 

Results  of  Simulation  Scenarios 

Case  1  -  Filter  Performance. 

The  initial  goal  in  reviewing  the  filter  results  was  to  ensure  that  the  filter  was 
successfully  filtering  the  collected  data.  To  verify  filter  performance,  the  residual  errors 
between  the  observed  data  and  the  reconstructed  trajectory  were  analyzed  to  ensure  that 
whenever  possible  they  had  a  nearly  zero  mean  error,  an  apparent  random  scattering  in 
error  about  the  mean  error,  and  a  low  standard  deviation  in  error. 

In  order  to  assess  the  performance  of  the  algorithm,  a  test  case  was  constructed 
with  a  single  target  reentering  which  is  observed  from  an  altitude  of  800  km  to  an  altitude 
near  impact.  The  observing  sensor  collected  range,  azimuth,  and  elevation  data  at  a  rate 
of  2  Hz.  An  overview  of  the  collection  geometry  is  presented  in  Figure  4.  The  target  is 
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initially  acquired  by  the  sensor  over  the  State  of  Maine  and  is  tracked  to  its  impact  in  the 


central  United  States. 


The  residual  errors  between  the  observed  data  and  the  reconstructed  state  vectors 
are  presented  in  Figure  5.  The  residuals  shows  the  characteristics  of  near  zero  mean 
error,  random  scattering  about  the  mean  error,  and  a  low  standard  deviation  of  error.  For 
this  case,  the  mean  error  was  -0.0032  m  in  range,  -0.00016  deg  in  azimuth,  and  0.0037 
deg  in  elevation.  The  residual  scattering  appears  to  be  nearly  random,  the  error  standard 
deviation  was  1.9  m  in  range,  0.016  deg  in  azimuth,  and  0.015  deg  in  elevation. 
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Figure  5  -  Single  sensor  observation  residuals 


The  results  presented  in  Figure  5  were  typical  of  other  scenarios  that  were  filtered. 
With  these  results  satisfying  the  criteria  for  the  filter  performance  that  were  being 
investigated,  the  subsequent  focuses  of  analysis  were  examined.  The  errors  in  the 
trajectory  estimate  from  the  truth  trajectory  are  presented  in  Figure  6"  and  Table  2  as 
reference  for  analysis  performed  in  the  subsequent  sections. 


‘  Acceleration  units  of  and  -TA  are  used  when  plotting  acceleration  values  and  errors,  respectively.  In 

sec ^  sec ^  A  ^  A  J 

axes  labels  these  units  are  labeled  km/ss  and  m/ss  for  clarity  when  displayed  in  the  MATLAB  font. 


34 


Z  (km) 


- Truth 

Com  pi 

t 

jted 

“if 

o 


.a 

0  50  100  150  200  250  300  350  400  450 


Time  After  Initialization  (sec) 


20 

0 


-20 


-40 


0  50  100  150  200  250  300  350  400  450 


Time  After  Initialization  (sec) 


- Truth 

Com  pi 

| 

it 
?  \ 

J 

a 

o 


20 
0 

-20 
-40 

0  50  100  150  200  250  300  350  400  450 

Time  After  Initialization  (sec) 


Figure  6  -  Trajectory  results  with  primary  sensor 


Table  2.  Trajectory  error  values  with  primary  sensor 


X  ( m ) 

Y  (m) 

Z  ( m ) 

•4(7) 

>4(7) 

*(7) 

*C“) 

*  (?) 

*  &) 

53.1 

78.5 

99.3 

2.4 

7.7 

8.1 

1.9 

2.9 

3.1 

Error  RMS  (m): 

79.3 

Error  RMS  (™): 

6.6 

Error  RMS  (™): 

2.7 

The  transition  of  the  filter  from  least  squares  to  the  Kalman  filter  can  be  easily 
observed  in  the  plots  of  trajectory  errors  from  the  truth  reference,  presented  in  Figure  6. 
The  least  squares  filter  achieves  a  random  scattering  of  error  with  a  much  smaller 
standard  deviation  than  the  Kalman  filter  does.  Despite  this  fact,  it  is  also  noted  that  the 
Kalman  filter  appears  to  do  a  better  job  of  minimizing  the  mean  error.  This  is  especially 
noticeable  in  the  position  errors  from  truth. 
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Regardless  of  these  errors,  the  estimated  trajectory  is  very  close  to  the  truth 
values.  With  these  estimates  determined,  the  ballistic  coefficient  can  be  computed  from 
the  sensed  acceleration  values  with  Equations  (57)  and  (58).  An  example  of  ballistic 

coefficient  values  computed  with  this  method  is  in  Figure  7. 

7000 

6000 

5000 

o> 

I  4000 

o 

a3 

6  3000 

o 

GO 

m  2000 

1000 

0 

120  100  80  60  40  20  0 

Altitude  (km) 

Figure  7  -  Estimated  ballistic  coefficient 

The  computed  ballistic  coefficient  in  Figure  7  is  typical  of  an  RV’s  ballistic 
coefficient  derived  from  measurements.  At  higher  altitudes,  the  ballistic  coefficient  is 
largely  unobservable  and  it  climbs  towards  its  actual  value  as  the  target  descends  in 
altitude  before  impact.  The  results  obtained  were  close  enough  to  the  truth  values  to 
conclude  that  the  ballistic  coefficient  was  being  properly  computed. 

Although  initially  implemented  as  optional,  the  backward  smoothing  pass, 
discussed  in  Chapter  III  -  Phase  2c,  was  eventually  deemed  to  be  necessary  for  optimal 
results.  A  comparison  of  acceleration  estimates  computed  with  and  without  the  smoother 
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pass  enabled  appears  in  Figure  8.  While  the  unsmoothed  values  appear  to  be  close  to  the 
truth,  their  mean  error  has  a  standard  deviation  of  6.9  (^)  compared  to  1.9  (jQ  for  the 
smoothed  estimates. 


Figure  8  -  Effects  of  smoother  pass  on  estimates 

The  benefits  of  the  smoother  pass  are  further  revealed  when  these  accelerations 
are  transformed  into  the  corresponding  ballistic  coefficient  estimates.  The  ballistic 
coefficient  estimates  from  the  smoothed  and  unsmoothed  estimates  appear  in  Figure  9. 
This  zoomed  view  highlights  the  errors  throughout  the  unsmoothed  results.  Whereas  the 
smoothed  accelerations  converge  to  the  truth  value,  the  unsmoothed  accelerations 
overshoot  the  truth  and  then  overcompensate  to  a  ballistic  coefficient  that  is  less  than  the 
truth.  For  these  reasons,  the  backward  smoother  pass  was  deemed  integral  to  achieving 
the  best  results. 
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Altitude  (km) 


Figure  9  -  Ballistic  coefficient  estimated  with  and  without  smoother  pass. 

To  further  validate  the  filter  performance,  scenarios  with  maneuvering  targets 
were  constructed  to  observe  the  filter’s  ability  to  model  non-ballistic  accelerations.  The 
maneuvers  performed  by  the  target  are  summarized  in  Table  3. 


Table  3.  Reentry  scenarios8 


SCENARIO 

REENTRY  MANEUVERS 

1 

None 

2 

Below  40  km  alt:  x  =  xgrav  —  ,000980665(a/t  —  40 km) 

3 

Below  40  km  alt:  x  =  xgrav  —  .000980665 (alt  —  40 km) 

Below  20  km  alt:  y  —  ygrav  —  2(.  000980665(a/t  —  20/cm)) 

Both  scenarios  2  and  3  involved  accelerations  in  the  x  direction  that  ramp  up  from 
zero  as  altitude  decreased.  Scenario  3  added  a  level  of  complexity  with  a  second 


s  These  reentry  scenarios  were  arbitrarily  defined 
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maneuver  in  the  y  direction  that  begins  at  a  lower  altitude.  This  maneuver  ramped  up 
more  rapidly  than  the  maneuver  in  the  x  direction. 

These  scenarios  were  run  through  the  filter  with  the  same  observing  sensor.  The 
results  showed  that  the  filter  was  capable  of  modeling  maneuvering  accelerations  and  had 
similar  errors  to  the  non-maneuvering  case.  Error  plots  are  presented  in  Figure  10  and 
Figure  1 1  while  a  summary  of  the  fits  are  in  Table  4  and  Table  5. 
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Figure  10  -  Trajectory  results  from  scenario  2,  maneuvering  target 


Table  4.  Trajectory  errors  from  scenario  2,  maneuvering  target 


X  (m) 

Y  ( m ) 

Z  (m) 

•4(7) 

>4(7) 

•4(7) 

*© 

*  © 

*  © 

39.6 

61.1 

63.5 

2.4 

7.5 

8.9 

1.8 

2.9 

4.0 

41 


Z  (km)  X  Error  (m) 


0.2 

0.1 

0 

J8  -0.1 

z 

%  -°-2 
< 

-0.3 

-0.4 

-0.5 


- Truth 

Com  pi 

t 

jted 

4 

1 

1  0.02 


- Truth 

Computed 

il 

i 

| 

i  ^ 
$ 

# 

it 
?  ■ 

0.4 


- Truth 

Compi 

1 

d- 

o 


_50t _ t _ t _ t _ t _ t _ t _ t _ t _ t 

0  50  100  150  200  250  300  350  400  450 

Time  After  Initialization  (sec) 


Figure  11  -  Trajectory  results  from  scenario  3,  maneuvering  target 


Table  5.  Trajectory  errors  from  scenario  3,  maneuvering  target 


X  ( m ) 

Y  (m) 

Z  ( m ) 

•4(7) 

>4(7) 

•4(7) 

*C“) 

*  (?) 

*  &) 

46.9 

88.9 

66.7 

2.5 

10.1 

8.7 

1.8 

4.6 

4.5 

The  last  discovery  made  that  affected  the  error  of  the  estimated  trajectory 
pertained  to  the  least  squares  sliding  window  portion  of  the  algorithm.  The  section  of  the 
filter  was  intended  to  provide  the  Kalman  filter  with  an  initial  guess  for  the  position  of 
the  target  during  the  maneuvering  portion  of  the  data.  After  analysis  of  the  estimates 
computed  with  the  LSSW  compared  to  those  computed  without  the  LSSW  filter  active,  it 
was  determined  that  the  LSSW  failed  to  improve  the  trajectory  results  in  every  scenario. 
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As  an  example  of  this  behavior,  the  results  of  the  baseline  scenario  are  presented  in  Table 


6  both  with  and  without  the  LSSW  active. 


Table  6.  Trajectory  errors  due  to  LSSW 


X  (m) 

Y  ( m ) 

Z  (m) 

*(t) 

*  (") 

*(7) 

*  (?) 

*  (?) 

*  (?) 

56.9 

148.7 

139.9 

2.7 

11.4 

9.6 

1.9 

3.8 

3.4 

Error  RMS  (m): 

122.4 

Error  RMS  (™): 

8.7 

Error  RMS  (JQ: 

3.1 

Baseline  Error: 

79.3 

Baseline  Error: 

6.6 

Baseline  Error: 

2.7 

Difference: 

+54.4% 

Difference: 

+31.8% 

Difference: 

+  14.8% 

Case  2  -  Filter  Performance  Improvements  Through  Additional  Sensors. 

In  order  to  investigate  performance  improvements  that  could  result  from  filtering 
data  from  multiple  sensors,  three  additional  sensor  locations  were  defined.  The  three 
additional  sensor  locations,  all  equidistant  from  the  impact  point,  are  described  in  Table  7 
and  displayed  graphically  in  Figure  12.  These  sensors  incorporate  the  same  random 
observation  noise  as  described  in  Table  1. 


Table  7.  Observing  sensors 


SENSOR 

CHARACTERISTIC 

1 

Approximately  Along  Reentry  Azimuth,  400km  from  Impact 

2 

Approximately  45°  to  Reentry  Azimuth,  400km  from  Impact 

3 

Approximately  90°  to  Reentry  Azimuth,  400km  from  Impact 
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Figure  12  -  Additional  sensor  collection  geometries 


The  results  of  incorporating  data  from  an  additional  sensor  at  location  1  are 
presented  in  Figure  13  and  a  summary  of  the  trajectory  errors  from  the  truth  are  presented 
in  Table  8.  In  general,  the  addition  of  a  second  sensor  along  the  reentry  azimuth  has  no 
positive  effect  on  the  results  of  the  trajectory  fit,  in  this  scenario.  In  fact,  the  results  are 
worse  than  those  achieved  with  the  primary  sensor  alone.  It  is  unclear  why  the  results  are 
as  degraded  as  they  are,  but  it  is  assumed  that  adding  a  sensor  at  location  1  adds  little 
observability  to  the  problem  beyond  what  the  primary  sensor  already  provides. 
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Figure  13  -  Trajectory  results  with  addition  of  sensor  1 


Table  8.  Combined  trajectory  errors  with  addition  of  sensor  1 


X  (m) 

Y  ( m ) 

Z  ( m ) 

*(t) 

^(t) 

>4(7) 

*G0 

*  (?) 

*  GO 

246.2 

443.0 

381.2 

7.8 

9.8 

17.3 

2.5 

3.8 

5.5 

Error  RMS  (m): 

366.1 

Error  RMS  (™): 

12.3 

Error  RMS  (™): 

4.1 

Baseline  Error: 

79.3 

Baseline  Error: 

6.6 

Baseline  Error: 

2.7 

Difference: 

+361.7% 

Difference: 

+86.4% 

Difference: 

+51.9% 

The  results  of  incorporating  data  from  an  additional  sensor  at  location  2  are 
presented  in  Figure  14  and  a  summary  of  the  trajectory  errors  from  the  truth  are  presented 
in  Table  9.  In  general,  the  addition  of  a  second  sensor  approximately  45  degrees  off  of 
the  reentry  azimuth  has  a  positive  effect  on  the  results  of  the  trajectory  fit.  The  position, 
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velocity,  and  acceleration  errors  are  all  i 
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Figure  14  -  Trajectory  results  with  addition  of  sensor  2 


Table  9.  Combined  trajectory  errors  with  addition  of  sensor  2 


X  (m) 

Y  (m) 

Z  (m) 

*(t) 

*(7) 

>4(7) 

*C“) 

*  &) 

*  © 

12.2 

109.2 

28.7 

1.8 

4.3 

3.8 

2.0 

2.0 

2.3 

Error  RMS  (m): 

65.6 

Error  RMS  (™): 

3.5 

Error  RMS  (”): 

2.1 

Baseline  Error: 

79.3 

Baseline  Error: 

6.6 

Baseline  Error: 

2.7 

Difference: 

-17.3% 

Difference: 

-47.0% 

Difference: 

-22.2% 

The  results  of  incorporating  data  from  an  additional  sensor  at  location  3  are 
presented  in  Figure  15  and  a  summary  of  the  trajectory  errors  from  the  truth  are  presented 
in  Table  10.  In  general,  the  addition  of  a  second  sensor  approximately  90  degrees  off  of 
the  reentry  azimuth  has  a  positive  effect  on  the  results  of  the  trajectory  fit,  similar  to  the 
results  achieve  with  additional  sensor  2.  The  position,  velocity,  and  acceleration  errors 
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are  all  improved  over  the  results  achieved  using  only  the  primary  sensor,  and  when 


averaged  are  slightly  better  than  those  achieved  with  additional  sensor  2. 
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Figure  15  -  Trajectory  results  with  addition  of  sensor  3 


Table  10.  Combined  trajectory  errors  with  addition  of  sensor  3 


X  (m) 

Y  (m) 

Z  ( m ) 

^(t) 

>4(7) 

^C“) 

^  &) 

*  © 

13.7 

102.9 

33.4 

1.9 

3.6 

3.2 

2.2 

1.8 

2.0 

Error  RMS  (m): 

63.0 

Error  RMS  (™): 

3.0 

Error  RMS  (”): 

2.0 

Baseline  Error: 

79.3 

Baseline  Error: 

6.6 

Baseline  Error: 

2.7 

Difference: 

-20.6% 

Difference: 

-54.5% 

Difference: 

-25.9% 

These  results  show  that  accuracy  can  be  improved  by  collecting  data  from  an 
additional  sensor,  although  there  are  geometry  considerations.  A  secondary  sensor  added 
along  the  reentry  azimuth  was  detrimental  to  the  accuracy  of  the  combined  results,  but  a 
secondary  sensor  located  either  45  or  90  degrees  to  the  reentry  azimuth  improved  the 
accuracy  of  the  results.  Alternative  scenarios  were  investigated  that  did  not  show  the 
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addition  of  a  sensor  along  the  reentry  azimuth  to  be  detrimental,  but  the  trend  of  better 
results  being  achieved  by  shifting  a  secondary  sensor  away  from  the  reentry  azimuth  was 
consistently  found  throughout  these  alternatives. 

This  was  anticipated  based  on  general  knowledge  of  data  filtering.  By  adding  a 
second  sensor  along  the  trajectory’s  azimuth,  there  is  little  added  information  that  was  not 
already  present  from  the  primary  sensor.  By  adding  that  sensor  orthogonal  to  the 
azimuth,  the  amount  of  new  information  added  to  the  filter  is  maximized. 

Case  3  -  Filter  Performance  Improvements  Through  Increased  Data  Rate. 

The  next  variation  to  the  standard  collection  scheme  is  the  modification  of  the 
data  rate  at  which  the  sensor  or  sensors  collect  observations.  For  these  cases,  the  rate  of 
collection  will  be  increased  from  2  to  3  Hz  in  order  to  investigate  what  effects  this  may 
have. 

The  first  scenario  that  will  be  modified  is  the  initial  scenario  whose  results  are 
presented  in  Figure  6  and  Table  2.  The  single,  primary  sensor  collects  data  at  the 
increased  rate;  the  results  of  this  are  presented  in  Figure  16  and  Table  11. 
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Figure  16  -  Trajectory  results  with  primary  sensor  at  increased  data  rate 


Table  11.  Trajectory  errors  with  primary  sensor  at  increased  data  rate 


X  (m) 

Y  (m) 

Z  (m) 

*(t) 

^(t) 

v‘(t) 

*C") 

^  &) 

*  © 

67.8 

107.5 

104.8 

2.0 

6.8 

5.0 

1.6 

3.3 

2.2 

Error  RMS  (m): 

95.1 

Error  RMS  (™): 

5.0 

Error  RMS  (™): 

2.5 

2Hz  Error: 

79.3 

2Hz  Error: 

6.6 

2Hz  Error: 

2.7 

Difference: 

+  19.9% 

Difference: 

-24.2% 

Difference: 

-7.4% 

The  single  sensor  results  show  improvements  in  velocity  and  acceleration  at  the 
increased  data  rate,  with  reduced  accuracy  in  position.  The  next  modified  scenario  will 
increase  the  data  rates  of  secondary  sensors  that  are  collecting  data  from  different 
locations  than  the  primary  sensor.  The  first  of  these  will  recreate  the  geometry  whose 
results  are  presented  in  Figure  13  and  Table  8  with  the  secondary  sensor  at  location  1. 
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The  results  of  this  geometry  with  the 


presented  in  Figure  17  and  Table  12. 
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Figure  17  -  Trajectory  results  with  addition  of  sensor  1  at  increased  rate 


Table  12.  Trajectory  errors  with  addition  of  sensor  1  at  increased  rate 


X  (m) 

Y  (m) 

Z  ( m ) 

*(’) 

vr(~) 

•4(t) 

M3) 

n(3) 

M3) 

237.6 

433.2 

367.5 

8.5 

11.3 

19.5 

3.1 

3.4 

9.0 

Error  RMS  (m): 

355.5 

Error  RMS  (™): 

13.9 

Error  RMS  (™): 

5.8 

2Hz  Error: 

366.1 

2Hz  Error: 

12.3 

2Hz  Error: 

4.1 

Difference: 

-2.9% 

Difference: 

+  13.0% 

Difference: 

+41.5% 

While  the  trajectory  results  still  show  a  significant  error  when  compared  to  the 
results  from  the  primary  sensor  alone,  the  results  show  mixed  improvement  with  respect 
to  those  obtained  when  a  sensor  at  location  1  operated  at  the  nominal  collection  rate.  In 
this  case,  the  position  error  is  improved  by  2.9%  while  the  velocity  and  acceleration 
errors  worsened  by  13.0%  and  41.5%,  respectively.  These  results  do  not  show  promise 
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for  an  increased  collection  rate  being  a  means  of  achieving  increased  accuracy,  but  with 
such  poor  errors  to  begin  with,  it  may  be  an  unsuitable  case  for  comparison. 

The  next  modified  scenario  will  operate  a  sensor  at  location  2,  similar  to  the 
results  presented  in  Figure  14  and  Table  9,  at  the  increased  data  rate  of  3  Hz.  Results 
from  this  scenario  are  presented  in  Figure  18  and  Table  13. 
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Figure  18  -  Trajectory  results  with  addition  of  sensor  2  at  increased  rate 


Table  13.  Trajectory  errors  with  addition  of  sensor  2  at  increased  rate 


X  ( m ) 

Y  (m) 

Z  ( m ) 

*(t) 

^(t) 

M?) 

^C“) 

*C“) 

^  © 

14.1 

104.4 

32.6 

1.8 

3.8 

2.4 

2.2 

2.6 

1.9 

Error  RMS  (m): 

63.7 

Error  RMS  (™): 

2.8 

Error  RMS  (”): 

2.3 

2Hz  Error: 

65.6 

2Hz  Error: 

3.5 

2Hz  Error: 

2.1 

Difference: 

-2.9% 

Difference: 

-20.0% 

Difference: 

+9.5% 

Similar  to  the  results  achieved  for  increasing  the  collection  rate  of  a  sensor  at 
location  1,  the  error  in  position  was  improved  and  the  error  in  acceleration  was  worsened 
by  increasing  the  collection  rate  of  a  sensor  at  location  2.  However,  unlike  the  previous 
scenario,  the  error  in  velocity  was  improved.  Error  in  position  and  velocity  were 
improved  by  2.9%  and  20.0%,  respectively,  while  error  in  acceleration  worsened  by 
9.5%.  This  reduction  in  acceleration  accuracy  is  much  less  than  the  41.5%  reduction  in 
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acceleration  accuracy  computed  from  the  previous  scenario,  but  it  is  unclear  why  the 
results  are  consistently  worse  in  acceleration  in  these  two  scenarios. 

The  last  modified  scenario  will  operate  a  sensor  at  location  3,  similar  to  the  results 
presented  in  Figure  15  and  Table  10.  Results  from  this  scenario  are  presented  in  Figure 
19  and  Table  14. 
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Figure  19  -  Trajectory  results  with  addition  of  sensor  3  at  increased  rate 


Table  14.  Trajectory  errors  with  addition  of  sensor  3  at  increased  rate 


X  (m) 

Y  (m) 

Z  (m) 

>4(7) 

*C“) 

M3) 

*  © 

11.8 

99.4 

28.1 

1.5 

3.0 

2.5 

2.0 

1.6 

1.8 

Error  RMS  (m): 

60.0 

Error  RMS  (™): 

2.4 

Error  RMS  (™): 

1.8 

2Hz  Error: 

63.0 

2Hz  Error: 

3.0 

2Hz  Error: 

2.0 

Difference: 

-4.8% 

Difference: 

-20.0% 

Difference: 

-10.0% 

Improving  on  the  results  achieved  for  increasing  the  collection  rate  of  sensor  2, 
the  results  from  increasing  the  collection  rate  of  sensor  3  show  improvement  in  position, 
velocity,  and  now  acceleration.  Errors  were  improved  by  4.8%  in  position,  20.0%  in 
velocity,  and  10.0%  in  acceleration.  The  review  of  these  four  scenarios  suggest  that  an 
increase  in  the  collection  rate  of  a  sensor  can  affect  the  resulting  trajectory  results  either 
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positively  or  negatively  and  seem  to  be  highly  influenced  by  the  collection  geometry. 
Further  analysis  into  this  phenomenon  could  be  carried  out  focusing  on  data  rates  other 
than  those  selected  here.  For  many  collectors,  data  rates  in  excess  of  10  or  20  Hz  are  not 
unheard  of,  and  could  show  great  improvement  over  the  values  presented  here. 

Case  4  -  Filter  Performance  Improvements  Through  Collection  Geometry. 

When  investigating  the  effects  of  increasing  the  rate  of  data  collection,  it  was 
noted  that  the  results  showed  significant  variation  depending  on  the  geometry  of  the 
collecting  sensor.  In  that  case,  the  addition  of  a  sensor  90  degrees  off  of  the  reentry 
azimuth  was  of  most  benefit  to  the  accuracy  of  the  trajectory  fit.  To  further  analyze  this 
case,  the  filter  is  rerun  for  the  sensors  located  at  locations  1,  2,  and  3  without  the  primary 
sensor.  The  results  of  these  three  fits  can  be  compared  to  discover  trends  in  accuracy 
based  solely  on  a  single  sensor’s  collection  geometry. 

The  first  scenario  reviewed  places  the  sensor  at  location  1,  along  the  reentry 
azimuth,  operating  at  the  standard  data  rate  of  2  Hz.  The  results  of  this  scenario  are 
presented  in  Figure  20  and  Table  15. 
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Figure  20  -  Trajectory  results  from  sensor  1 

Table  15.  Trajectory  errors  from  sensor  1 


X  (m) 

Y  ( m ) 

Z  ( m ) 

>4(7)  *v  (=■) 

>4(7) 

*  &)  *  &) 

*C“) 

57.5 

163.5 

107.6 

2.7  6.2 

6.8 

2.0  2.9 

3.3 

Error  RMS  (m): 

117.8 

Error  RMS  (™): 

5.5 

Error  RMS  (™): 

2.8 

The  results  from  this  scenario  will  be  used  as  a  baseline  for  the  results  achieved 
when  the  sensor  is  placed  at  locations  2  and  3.  Deviations  from  these  results  will  be  used 
to  determine  whether  collection  geometry  has  a  noticeable  effect  on  the  accuracy  of  the 
reconstructed  trajectories. 
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The  next  scenario  places  the  sensor  at  location  2,  approximately  45  degrees  off  of 
the  reentry  azimuth,  operating  at  the  standard  data  rate  of  2  Hz.  The  results  of  this 
scenario  are  presented  in  Figure  21  and 
Table  16. 
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Figure  21  -  Trajectory  results  from  sensor  2 

Table  16.  Trajectory  errors  from  sensor  2 


X  ( m )  Y  (m) 

Z  (m) 

*  (7)  *  (7) 

*(7) 

*  (7)  *  (3) 

M3) 

25.3  36.4 

24.9 

9.8  4.1 

5.7 

5.1  1.9 

3.2 

Error  RMS  (m): 

29.4 

Error  RMS  (™): 

7.0 

Error  RMS  (™): 

3.6 

Baseline  Error: 

117.8 

Baseline  Error: 

5.5 

Baseline  Error: 

2.8 

Difference: 

-75.0% 

Difference: 

+27.3% 

Difference: 

+28.6% 

With  the  sensor  at  location  2,  the  position  accuracy  was  improved  by  75.0%  while 
the  velocity  and  acceleration  results  both  suffered  degradations  in  accuracy,  when 
compared  to  the  truth  reference,  of  27.3%  and  28.6%,  respectively. 

The  other  scenario  that  was  tested  involved  the  placement  of  a  sensor  at  location 
3,  approximately  perpendicular  to  the  reentry  azimuth,  operating  at  the  standard  data  rate 
of  2  Hz.  The  results  from  this  scenario  are  presented  in  Figure  22  and 
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Figure  22  -  Trajectory  results  from  sensor  3 

Table  17.  Trajectory  errors  from  sensor  3 


X  ( m )  Y  (m) 

Z  ( m ) 

*  (?)  >v  (?) 

*(?) 

*  (?)  *  (?) 

*  (?) 

39.6  75.4 

58.6 

5.3  4.8 

2.5 

3.0  2.4 

1.7 

Error  RMS  (m): 

59.7 

Error  RMS  (™): 

4.4 

Error  RMS  (™): 

2.4 

Baseline  Error: 

117.8 

Baseline  Error: 

5.5 

Baseline  Error: 

2.8 

Difference: 

-49.3% 

Difference: 

-20.0% 

Difference: 

-14.3% 

As  in  the  previous  scenario,  the  position  accuracy  was  improved  over  the 
baseline,  this  time  showing  a  49.3%  improvement.  Furthermore,  the  velocity  and 
acceleration  results  for  this  scenario  showed  an  improvement  in  accuracy.  Velocity  and 
acceleration  errors,  when  compared  to  the  truth  reference,  were  reduced  by  20.0%  and 
14.3%,  respectively. 
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Reviewing  these  scenarios  collectively,  it  is  noted  that  position  errors  were 
reduced  in  both  cases  where  the  sensor  was  not  operating  at  a  location  along  the  reentry 
azimuth.  However,  while  the  position  errors  showed  a  trend  that  encouraged  the 
placement  of  the  sensor  farther  from  the  reentry  azimuth,  the  results  in  velocity  and 
acceleration  were  mixed  and  may  require  further  analysis  to  investigate  subtle  trends. 

Investigative  Questions  Answered 

After  validating  the  filter’s  performance  against  different  scenarios,  several 
scenario  adjustments  were  investigated  to  determine  any  benefits  that  could  be  derived. 
Initially,  it  was  determined  that  including  observations  from  a  second  sensor  could 
improve  the  filter’s  accuracy,  but  this  varied  with  the  placement  of  the  second  sensor. 
The  greatest  improvement  was  derived  from  placing  the  sensor  perpendicular  to  the 
reentry  azimuth  of  the  target. 

After  investigating  the  effects  of  a  second  sensor,  the  data  rate  of  the  collecting 
sensors  were  varied.  It  was  determined  that  increasing  the  data  rate  of  either  the  primary 
or  secondary  sensors  could  improve  the  filter’s  accuracy.  This  result  was  expected  as  it 
increases  the  filter’s  knowledge  of  the  target  during  the  same  time  period. 

The  final  scenario  modification  involved  further  investigation  of  sensor  geometry. 
When  adding  additional  sensors  to  the  primary  collector,  it  was  noted  that  the  location  of 
the  second  sensor  could  vary  the  resulting  filter  accuracy.  This  phenomenon  was  further 
investigated  by  considering  data  only  from  the  secondary  sensor  at  reduced  range.  The 
sensor  was  operated  at  the  three  different  collection  locations  and  the  resulting  filter 
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accuracy  was  reviewed.  In  the  scenarios  investigated,  the  filter  achieved  its  best  accuracy 
when  the  single  sensor  was  operated  perpendicular  to  the  target’s  reentry  azimuth. 

Summary 

This  chapter  investigated  the  accuracy  of  the  filter  by  comparing  its  output  to 
truth  data  used  to  generate  the  observations  that  were  fed  to  the  filter.  Several  different 
scenarios  were  investigated,  including  adding  a  second  sensor,  increasing  the  data 
collection  rate,  and  changing  the  sensor  collection  geometry.  The  results  of  these 
modifications  were  reviewed  and  summarized. 
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V.  Conclusions  and  Recommendations 


Chapter  Overview 

This  chapter  will  cover  the  general  conclusions  that  were  drawn  from  the  analysis 
section  of  the  paper.  Additionally,  recommendations  for  future  action  and  research  are 
presented. 

Conclusions  of  Research 

The  filter  developed  for  this  thesis  combined  the  strengths  of  the  least  squares  and 
Kalman  filters.  The  least  squares  filter  operates  rapidly  and  accurately  on  the  free-flight 
portions  of  flight.  The  Kalman  filter  provides  greater  flexibility  for  the  state’s 
acceleration  components  to  vary  lower  in  the  atmosphere.  The  filter  successfully 
transitioned  from  the  least  squares  to  Kalman  filter,  using  the  final  values  of  the  free 
flight  propagation  for  the  Kalman  filter’s  initial  state. 

The  developed  algorithm  includes  a  least  squares  sliding  window  filter  that 
estimates  an  initial  guess  of  position  during  the  maneuvering  phase  of  flight.  After 
investigation  of  the  effects  of  computing  these  initial  guesses,  it  was  determined  that  they 
consistently  had  a  detrimental  effect  on  the  filter  estimates.  After  this  was  concluded,  the 
least  squares  sliding  window  was  not  implemented  for  the  results  presented  in  Chapter 
IV. 

Once  the  filter  was  validated  against  both  maneuvering  and  non-maneuvering 
targets,  the  filter  was  used  to  investigate  other  collection  scenario  modifications  and  their 
effects.  The  filter  achieved  varying  levels  of  accuracy  when  the  scenario  was  modified 
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with  a  different  number  of  sensors,  increasing  data  collection  rates,  and  different 
collection  geometries. 

After  investigating  these  scenarios,  it  was  determined  that  the  best  results  were 
achieved  with  additional  collectors,  by  increasing  the  data  collection  rate,  and  by  moving 
the  collector  position  perpendicular  to  the  reentry  azimuth. 

Significance  of  Research 

This  research  expands  on  the  work  of  previous  Air  Force  Institute  of  Technology 
(AFIT)  graduate  students’  work  in  the  area  of  data  filtering  of  collections  of  reentry 
vehicles.  Whereas  previous  research  only  addressed  non-maneuvering  or  simple 
maneuvering  targets  with  fixed  bank  angles,  this  work  allows  for  the  study  of  complex 
maneuvering  targets  with  varying  accelerations. 

Of  further  significance,  the  filter  algorithm  allows  for  the  inclusion  of  multiple 
sensors.  Including  all  of  the  available  data  into  the  filter  estimates  ensures  that  the  best 
results  can  be  achieved. 

Recommendations  for  Action 

While  several  scenarios  were  investigated  to  determine  the  effects  of 
modifications  to  the  number  of  collectors,  their  data  rates,  and  their  collection  geometry, 
more  work  could  be  done  to  further  investigate  these  areas.  The  accuracy  of  the  filter 
results  varied  significantly  in  all  scenarios,  so  subtler  investigation  could  identify  trends 
and  true  optimums.  Error  contour  plots  could  be  generated  for  different  scenarios  in 
order  to  better  illustrate  results. 
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Additionally,  further  modifications  could  be  investigated  beyond  those  introduced 
here.  The  algorithm  was  written  to  accommodate  multiple  targets  simultaneously,  but  is 
not  investigated  here.  Another  major  area  that  could  be  investigated  is  the  benefit  of 
range,  azimuth,  and  elevation  sensors  over  sensors  that  only  measure  azimuth  and 
elevation.  The  algorithm  is  written  to  manage  both  of  these  types  of  sensors  and  this 
could  be  a  major  area  of  investigation. 

Recommendations  for  Future  Research 

As  discussed  in  the  filter  performance  section  of  the  analysis  and  conclusions,  the 
least  squares  filter  is  not  performing  as  well  on  the  non-maneuvering  portion  of  the 
trajectory  as  the  Kalman  filter  is  performing  on  the  lower  regions  of  flight.  There  may  be 
ways  of  improving  this  performance.  Areas  to  investigate  could  include  modifications  to 
the  numerical  partial  derivatives  that  are  used.  These  could  either  be  altered  with 
different  perturbation  sizes  or  replaced  by  analytical  solutions  to  the  partials.  One 
possibility  could  be  adjusting  the  perturbation  magnitudes  as  the  targets  approach 
convergence.  If  this  portion  of  the  filter  performed  better,  the  overall  performance  would 
likely  be  greatly  improved. 

Other  modifications  that  could  be  researched  include  the  inclusion  of  further  data 
types.  The  algorithm  as  written  addresses  two  data  types:  range,  azimuth,  and  elevation 
sensors  and  azimuth  and  elevation-only  sensors.  There  are  situations  where  additional 
data  types  could  be  available  and  it  would  be  a  benefit  to  incorporate  these  data  types  into 
the  analysis.  One  easy  data  type  that  could  be  included  would  be  GPS  or  other  position 
information  obtained  from  the  operator  of  the  test.  Another  data  type  that  could  be 
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included  would  be  right-ascension  and  declination  angle  measurements  that  are  typical  of 
overhead  sensors.  This  data  type  could  easily  be  incorporated  due  to  its  similarity  to  the 
azimuth  and  elevation  data  type. 

Summary 

This  chapter  reviewed  the  research  and  offered  some  general  conclusions  that 
were  derived  and  suggested  future  work  that  could  be  performed.  In  the  previous 
chapters,  the  filter  is  derived  and  validated  against  various  scenarios.  Future  work  could 
be  done  in  either  the  areas  of  collection  optimization  using  the  filter  as  it  currently  exists, 
or  filter  modification  to  either  improve  performance  or  accommodate  data  types  from 
sensor  types  that  were  not  considered  for  this  thesis. 
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Appendix 


Coordinate  Frames 

All  computation  is  done  in  an  Earth-Centered  Inertial  (ECI)  reference  frame  when 
possible,  most  notably  in  the  EOMs  which,  when  expressed  in  ECI  are  simplified  to  those 
in  Equations  (6-11).  When  this  is  not  possible,  as  in  the  case  of  Equation  (32),  it  is 
important  to  realize  the  coordinate  frames  being  referenced.  This  section  does  not  define 
the  algorithms  to  transform  from  one  frame  to  another,  but  simply  defines  the  reference 
frames. 


Figure  23  -  Earth-Centered  Inertial  (ECI)  coordinate  frame 
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Figure  25  -  Latitude/Longitude  coordinate  frame 


75 


kaliper.m 

kaliper  .  m  is  the  main  filter  code  written  in  the  MATLAB  scripting  language. 
Original  formatting  is  preserved  to  maintain  functionality  when  pasted  into  MATLAB. 


function  varargout  =  kaliper (varargin) 

%KALIPER  -  Kalman  filter  And  Least  squares  Integrated  Parameter 
Estimation  Routine 


%  --  Usage  -- 
%  state 

%  [state,  target] 

%  [state,  target,  stats] 


%  --  Input 
%  target  - 
%  target 
%  target 
%  target 
%  target 
(numobs  x 
%  target 
%  target 
%  target 


definition  -- 
data  structure 
obs 

obs  time 
obs_snr 
obs  f f 


{ tgt } 
{ tgt } 
{tgt} 
{tgt} 
1) 

{tgt} 

{tgt} 

{tgt} 


.  numobs 
.  init  time 
.  init  sv 


kaliper (target,  sensor,  koptions) 
kaliper (target,  sensor,  koptions) 
kaliper (target,  sensor,  koptions) 


defining  observed  targets  of  the  form: 

-  metric  observations  (numobs  x  3) 

-  metric  observation  times  (numobs  x  1) 

-  indices  of  sensors  for  all  obs  (numobs  x  1) 

-  logical  array  specifying  free  flight  obs 

-  number  of  metric  observations  (1) 

-  time  of  initial  guess  state  vector 

-  initial  guess  state  vector  (1x10) 


sensor  -  data  structure  defining  observing  sensors  of  the  form: 


sensor { snr }. snr  type 


3) 

o, 

o 

o, 

o 

3) 

o, 

o 

1) 

o, 

o 

x  9) 


sensorfsnr} ,pos_lla 

sensorfsnr} ,pos_ecf 

sensor { snr }. pos  time 

sensorfsnr} . tm 

sensor{snr} .stddev 
sensor { snr }. obs  bias 


sensor  type,  one  of: 

1:  stationary  ranged  (range, az, el) 

2:  stationary  two-angle  (az,el) 

3:  moving  range  (range, az, el) 

4:  moving  two-angle  (az,el) 

LLA  sensor  position  (1x3)  OR  (numeph  x 

(geod  lat.  Ion,  alt)  -  (rad,  rad,  km) 
ECEF  sensor  position  (1x3)  OR  (numeph  x 

moving  sensor  ephemeris  times  (numeph  x 

SEZ  to  ECEF  rot  matrix  (3x3)  OR  (numeph 

standard  deviations  of  obs  (1x3) 
constant  observation  biases  (1x3) 


%  koptions  -  data  structure  defining  kaliper  run  options  (Optional) 

%  koptions . echo  -  true/false  flag  to  echo  status  to 

command  (1) 

%  {default  value  =  false (1)} 

%  koptions. sig  edit  -  number  of  std  deviations  for  editting 

( 1 ) 

%  {default  value  =  3} 
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%  koptions.max  iter 
iterations  (1) 


koptions . conv  tol 


(1) 


%  koptions . lssw 
KF  (1) 

o, 

o 

%  koptions . window  size 

pass  (1) 

o, 

o 

%  koptions. min  window  size 

initialization  (1) 

Q. 

O 

%  koptions.max  window  size 
termination  (1) 


(1) 


(1) 


koptions . qp 


koptions . qv 


%  koptions. qa 
factor  (1) 


maximum  number  of  least  squares 
{default  value  =  40} 

convergence  tolerance  for  least  squares 
{default  value  =  .0001} 

true/false  flag  to  use  LSSW  results  in 

{default  value  =  true(l)} 

sliding  window  nominal  size  for  LSSW 

{default  value  =  30} 

sliding  window  minimum  size  for  LSSW 
{default  value  =  5} 

sliding  window  maximum  size  for  LSSW 
{default  value  =  40} 

KF  position  plant  noise  scale  factor 
{default  value  =  0.0} 

KF  velocity  plant  noise  scale  factor 

{default  value  =  0.0} 

KF  acceleration  plant  noise  scale 

{default  value  =  0.001} 


--  Output  definition  -- 

state  -  data  structure  defining  state  vector  components 


%  state { tgt }. time 
%  state { tgt } . sv 
%  {1-3} 

day  of  collect 
%  {4-6} 

%  {7-9} 

accel  -  gravity) 

%  state { tgt }. sv_smooth 
x  9) 

%  {1-3} 

day  of  collect 
%  {4-6} 

%  {7-9} 


metric  observation  times  (numobs  x  1) 
state  vector  solution  (numobs  x  9) 

ECI  positions  (km),  ECI  epoch  @  0  GMT 

ECI  velocities  (km/s) 

ECI  sensed  accelerations  (km/s2)  (total 
smoothed  state  vector  solution  (numobs 
ECI  positions  (km) ,  ECI  epoch  @  0  GMT 
ECI  velocities  (km/s) 

ECI  sensed  accelerations  (km/s2)  (total 


%  accel  -  gravity) 

o, 

o 

%  target  -  if  requested,  target  structure  is  returned  with  updated 
parameters 


%  stats  -  data  structure  detailing  estimation  statistics 

%  stats { iter , tgt }. cost_func  -  Weighted  cost  function  of  target  by 

iteration 

%  stats { iter , tgt }. cov  -  covariance  update  computed  by  target 

and  iteration 


78 


%  stats { iter , tgt }. res { snr }  -  residual  vectors  by  iteration,  target, 

and  sensor 

%  variables  persistent  to  this  function  for  subsequent  calls 
persistent  target  sensor  koptions  earth 

%  define  earth  parameters  for  later  use 
earth  =  define  earth; 

%  define  atmospheric  parameters 
read_stdatmos ( ' stdatmos7  6 . dat ' ) 

%  accept  input 
if  nargin  ==  2 

target  =  varargin{l}; 
sensor  =  varargin{2}; 

%  options  undefined,  will  be  filled  with  defaults 
koptions  =  []; 
elseif  nargin  ==  3 

target  =  varargin{l}; 
sensor  =  varargin{2}; 
koptions  =  varargin{3}; 

else 

error (' Input  should  either  be  target  &  sensor  or  target,  sensor, 
and  koptions ' ) 
end 

%  fill  undefined  components  of  the  options  structure  with  default 
values 

if  isempty ( koptions ) 

koptions . echo  =  false  (1); 
koptions . sig_edit  =  3; 
koptions. max  iter  =  40; 
koptions . conv  tol  =  .0001; 
koptions . lssw  =  true(l); 
koptions . window  size  =  30; 
koptions. min  window  size  =  5; 
koptions. max  window  size  =  40; 
koptions. qp  =  0; 
koptions. qv  =  0; 
koptions. qa  =  0.001; 

else 

if  -isfield ( koptions ,  'echo')  ||  isempty (koptions .echo) 
koptions . echo  =  false  (1); 

end 

if  -isfield ( koptions ,  'sig_edit')  I!  isempty (koptions . sig_edit) 
koptions . sig_edit  =  3; 

end 

if  -isfield ( koptions ,  'max  iter')  | |  isempty (koptions .max  iter) 
koptions. max  iter  =  40; 

end 

if  -isfield ( koptions ,  'conv  tol')  | |  isempty ( koptions . conv  tol) 
koptions . conv_tol  =  .0001; 

end 
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if  ~isf ield ( koptions ,  ' lssw')  | |  isempty ( koptions . lssw) 

koptions . lssw  =  true(l); 

end 

if  ~isf ield ( koptions ,  'window  size')  |  | 
isempty ( koptions . window  size) 

koptions . window  size  =  30; 

end 

if  ~isfield (koptions,  'min  window  size')  | 
isempty (koptions .min  window  size) 

koptions. min  window  size  =  5; 

end 

if  ~isf ield ( koptions ,  'max  window  size')  | 
isempty (koptions .max  window  size) 


koptions. max  window  size 

=  40; 

end 

if  ~isf ield ( koptions ,  ' 

qp')  1 

|  isempty ( koptions . qp) 

koptions. qp  =  0; 

end 

if  ~isf ield ( koptions ,  ' 

qv') 

isempty ( koptions . qv) 

koptions. qv  =  0; 

end 

if  ~isf ield ( koptions ,  ' 

qa '  )  | 

isempty ( koptions  .  qa) 

koptions. qa  =  0.001; 

end 


end 

%  optional  plot  of  residuals 
if  koptions . echo 
figure; 

resaxl  =  subplot  (3, 1 , 1 )  ; 
resax2  =  subplot (3, 1 , 2 )  ; 
resax3  =  subplot (3, 1 , 3) ; 

end 

%  number  of  targets  in  structure 
numtgt  =  length (target) ; 

%  time  pad  to  add  to  observations  at  the  same  time  value 
time_pad  =  le-10; 

%  loop  through  targets 
for  tgt  =  1: numtgt 

%  number  of  observations  of  this  target 
numobs  =  length (target { tgt }. obs_time) ; 

%  number  of  sensors  for  this  target 
tgtsnr  =  unique (target { tgt }. obs_snr) ; 
numsnr  =  length (tgtsnr)  ; 

%  add  small  time  intervals  on  to  any  observations  at  the  same  time 
repeat_idx  =  diff (target { tgt }. obs_time)  ==  0; 
repeat_idx  =  [false (1);  repeat_idx] ; 

%#ok<AGROW> 
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0; 


while  any (repeat  idx) 

target { tgt } . obs_time ( repeat^idx)  = 
target { tgt }. obs^time ( repeat_idx)  +  time_pad; 

repeat_idx  =  diff (target { tgt }. obs_time)  == 
repeat_idx  =  [false (1);  repeat_idx] ; 

%#ok<AGROW> 
end 

%  convert  3-D  observations  to  ECI  frame  for  SV  initial  guess 
target { tgt }. obs  eci  =  repmat (NaN,  numobs,  3);  %  NaN  padding 

%  locate  3-D  observations  of  stationary  and  moving  sensors 
for  snridx  =  l:numsnr 

%  index  of  this  sensor 
snr  =  tgtsnr ( snridx) ; 

%  observations  from  this  sensor,  and  those  observation  times 
obsidx  =  target { tgt }. obs_snr  ==  snr; 
obs_time  =  target { tgt }. obs^time (obsidx) ; 

%  stationary  range,  az,  el  observations 
if  sensor { snr }. snr  type  ==  1 

obs_sez  =  tm_rae2sez (target { tgt }. obs (obsidx,  :))  ; 
obs_ecf  =  tm_sez2ecr (obs_sez,  sensor { snr }. pos_ecf, 
sensor {snr} . tm) ; 

%  moving  range,  az,  el  observations 
elseif  sensor { snr }. snr  type  ==  3 

obs_sez  =  tm_rae2sez (target { tgt }. obs (obsidx, :)) ; 

%  interpolate  sensor  positions  at  observation  times 
interp  lat  =  interpl ( sensor { snr }. pos  time, 
sensor { snr }. pos_lla (:, 1 ) ,  obs_time,  'spline'); 

interp  Ion  =  interpl ( sensor { snr }. pos  time, 
sensor { snr }. pos_lla (;, 2 ) ,  obs_time,  'spline'); 

interp  alt  =  interpl ( sensor { snr }. pos  time, 
sensor { snr }. pos_lla (:, 3 ) ,  obs_time,  'spline'); 

[sensor  pos,  sensor  tm]  =  calc  sensor  move (interp  lat, 
interp  Ion,  interp  alt) ; 

%  moving  transformation 

obs  ecf  =  tm  sez2ecr  move (obs  sez,  sensor  pos,  sensor  tm) 

else 

%  sensor  without  3-D  observation,  move  to  next  sensor 
continue; 

end 

%  convert  ecef  to  eci 

obs_eci  =  tm_ecr2eci (obs_ecf ,  0,  obs_time,  0) ; 

%  insert  transformation  into  structure 
target { tgt }. obs_eci (obsidx, ; )  =  obs_eci; 

end 
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%  check  if  initial  guess  has  been  specified,  otherwise  compute  it 
if  -isfield (target { tgt } ,  ' init_sv')  | 

-isempty (target { tgt } . init_sv) 

[init  time,  init  sv]  =  kaliper  calcinit (tgt) ; 
target { tgt }. init  time  =  init  time; 
target { tgt }. init_sv  =  init_sv; 

end 

end 


oo.  oo 
o  o  o  o 

%%%%  Phase  1  state  vector  estimation  -  Free  Flight  -  Sensed  Accels  =  0 

oo  oo 
o  o  o  o 


if  koptions . echo 

disp('**  Entering  Phase  1  -  Free-Flight  Estimation  **') 

end 

%  set  free  flight  ode45  options 

options  =  odeset ( ' RelTol ' ,  le-6,  'Vectorized',  'on'); 

%  initialize  least  squares  run 
ls_iter  =  1; 

converged  =  false (size (target) ) ; 

%  begin  estimation 

while  any ( ~converged)  &&  Is  iter  <=  koptions. max  iter 
if  koptions . echo 

disp(['  *  Iteration  '  num2str (ls_iter) ] ) 

end 

for  tgt  =  l:numtgt 
if  koptions . echo 

disp(['  -Target  '  num2str (tgt) ] ) 

end 

%  initialize  running  sums  for  this  target 
TtQiT  =  zeros ( 6) ; 

TtQir  =  zeros (6,1); 
cost_func  =  0; 

%  time  series  to  propagate  to,  first  value  is  time  of  SV 
prop_time  =  [target { tgt }. init_time; 
target { tgt } . obs_time (target { tgt } . obs_f f ) ] ; 

if  prop_time(l)  ==  prop_time (2 ) ,  prop_time(l)  =  [];  end 

%  propagate  SV  to  all  FF  observation  times,  ignoring  any  sensed 
accelerations 

[calc_time, calc_sv]  =  ode45 (@kaliper_eom,  prop_time, 
target { tgt }. init^sv ' ,  options); 
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%  propagate  perturbed  SVs  for  numerical  partial  derivates  to 
form  H  matrix 

sv_pert  =  [.001  .001  .001  .0001  .0001  .0001];  %  accelerations 
not  perturbed 

pert_init_sv  =  repmat (target { tgt }. init_sv,  6,  1)  + 

[diag (sv_pert)  zeros (6,  3)]; 

[pert_time, pert_sv]  =  ode45 (@kaliper_eom,  prop_time, 
pert_init_sv ' ,  options) ; 

%  transform  propagated  SVs  to  sensor  data  reference,  compute 
TtQiT  and  TtQir 

for  snridx  =  l:numsnr 


%  index  of  this  sensor 
snr  =  tgtsnr ( snridx) ; 

%  observations  from  this  sensor,  and  those  observation 

times 

obsidx  =  target { tgt }. obs_snr (target { tgt }. obs_ff)  ==  snr; 
%obs_time  = 

target { tgt } . obs^time (target { tgt } . obs_f f (obsidx) ) ; 

obs_time  =  target { tgt }. obs_time (target { tgt }. obs_ff) ; 
obs  time  =  obs  time (obsidx) ; 
numobs  =  length (obs  time); 

if  numobs  ==  0,  continue,  end 


%  stationary  range,  az,  el  OR  az,  el  observations 
if  sensor { snr }. snr  type  ==  1  | |  sensor { snr }. snr  type  ==  2 

%  convert  ECI  SV  positions  to  stationary  RAE 
calc_eci  =  calc_sv (obsidx, 1 : 3) ; 

calc_ecf  =  tm_eci2ecr (calc_eci,  0,  obs_time,  0) ; 
calc_sez  =  tm_ecr2sez (calc_ecf ,  sensor { snr } ,pos_ecf, 

sensor {snr} . tm) ; 

calc  rae  =  tm  sez2rae(calc  sez) ; 


%  same  for  perturbed  states 
pert_eci  =  pert_sv (obsidx, ;) ; 

pert_eci  =  reshape (pert_eci '  ,  9,  []) ';  %  1  vec  /  row 

pert_ecf  =  tm_eci2ecr (pert_eci ( ; , 1 ; 3 )  ,  0,  ... 

reshape (repmat (obs_time,  1,  6)',  [],  1),  0)  ; 

pert_sez  =  tm_ecr2sez (pert_ecf ,  sensor { snr }. pos_ecf, 

sensor {snr} .tm) ; 

pert_rae  =  tm_sez2rae (pert_sez ) ; 
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%  moving  range,  az,  el  OR  az,  el  observations 
elseif  sensor { snr }. snr  type  ==  3  |  sensor { snr }. snr  type 


%  interpolate  sensor  positions  at  observation  times 
interp  lat  =  interpl ( sensor { snr }. pos  time, 
sensor { snr }. pos_lla (:, 1 ) ,  obs_time,  'spline'); 
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interp  Ion  =  interpl ( sensor { snr }. pos  time, 
sensor { snr }. pos_lla (:, 2 ) ,  obs_time,  'spline'); 

interp  alt  =  interpl ( sensor { snr }. pos  time, 
sensor { snr }. pos_lla (;, 3 ) ,  obs_time,  'spline'); 

[sensor  pos,  sensor  tm]  =  calc  sensor  move (interp  lat, 
interp  Ion,  interp  alt) ; 

%  convert  ECI  SV  positions  to  moving  RAE 
calc_eci  =  calc_sv (obsidx, 1 : 3) ; 

calc_ecf  =  tm_eci2ecr (calc_eci,  0,  obs_time,  0)  ; 
calc  sez  =  tm  ecr2sez  move (calc  ecf,  sensor  pos, 

sensor__tm)  ; 

calc_rae  =  tm_sez2rae (calc_sez) ; 

%  same  for  perturbed  states 
pert_eci  =  pert_sv (obsidx, :) ; 

pert_eci  =  reshape (pert_eci ' ,  9,  []) ';  %  1  vec  /  row 

pert_ecf  =  tm_eci2ecr (pert_eci ( : , 1 : 3 ) ,  0,  ... 

reshape (repmat (obs^time,  1,  6)',  [],  1),  0)  ; 

pert  sez  =  tm  ecr2sez  move (pert  ecf,  sensor  pos, 

sensor^tm) ; 

pert_rae  =  tm_sez2rae (pert_sez ) ; 


else 

error ([' Unrecognized  sensor  type  ID;  ' 
num2str (sensor { snr }. snr  type)]) 
end 

%  use  calculated  RAE  values  to  compute  residuals  vectors 
if  sensor { snr }. snr  type  ==  1  | |  sensor { snr }. snr  type  ==  3 

%  range,  azimuth,  and  elevation  observations 
obs_rae  =  target { tgt }. obs (target { tgt }. obs_ff, :) ; 
res  =  obs_rae (obsidx, : )  -  calc_rae; 

%  save  residuals 

stats { ls_iter, tgt }. res { snr }  =  res; 

%#ok<AGROW> 


%  plot  residuals 
if  koptions . echo 
if  snr  ==  1 

cla(resaxl),  plot(resaxl,  obs_time,  res(:,l), 
'r.'),  hold(resaxl,  'on') 

cla(resax2),  plot(resax2,  obs_time,  res(:,2), 
'r.'),  hold(resax2,  'on') 

cla(resax3),  plot(resax3,  obs_time,  res(:,3), 
'r.'),  hold(resax3,  'on') 

drawnow 

else 

cols  =  ' rbmk ' ;  nc  =  length (cols) ; 
plot(resaxl,  obs_time,  res(;,l),  [cols (mod (snr 

l,nc)+l)  '.']) 

plot(resax2,  obs_time,  res(;,2),  [cols (mod (snr 

1 , nc) +1 )  '.']) 
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1 , nc) +1 ) 


plot(resax3,  obs_time,  res(:,3),  [cols (mod (snr 


drawnow 

end 

end 


%  statistical  editting  of  outliers  based  on  sensor  std 

dev 

if  ls_iter  >  1 

res (  abs ( res  ( : , 1 ) )  > 

sensor { snr }. stddev ( 1 ) *koptions . sig_edit,  1)  =  0; 

res (  abs ( res  ( : , 2 ) )  > 

sensor { snr }. stddev (2 ) *koptions . sig_edit,  2)  =  0; 

res (  abs ( res  ( : , 3 ) )  > 

sensor { snr }. stddev (3) *koptions . sig_edit,  3)  =  0; 
end 


%  mark  editted  points 
if  koptions . echo 

editl  =  abs ( stats { ls_iter, tgt }. res { snr }(:,  1 )  )  > 
sensorfsnr} . stddev ( 1 ) * koptions . sig_edit ; 

edit2  =  abs ( stats { ls_iter , tgt } . res { snr }  ( : , 2 ) )  > 
sensorfsnr} . stddev (2 ) * koptions . sig_edit ; 

edit3  =  abs ( stats { ls_iter , tgt } . res { snr }  ( : ,  3 )  )  > 
sensor { snr} . stddev (3) *koptions . sig_edit; 

if  any (editl),  plot(resaxl,  obs_time (editl ) , 
stats { ls_iter, tgt }. res { snr } (editl , 1 ) ,  ' kx ' ) ,  end 

if  any(edit2),  plot(resax2,  obs_time (edit2 ) , 
stats { ls_iter , tgt } . res { snr } (edit2 , 2 ) ,  ' kx ' ) ,  end 

if  any(edit3),  plot(resax3,  obs_time (edit3 ) , 
stats { ls_iter , tgt } . res { snr } (edit3 , 3 ) ,  ' kx ' ) ,  end 

end 


%  cost  function  update 
cost_func  =  cost_func  +  ... 

sqrt (  sum(  res(:,l).A2  )  )  /  sensor { snr }. stddev ( 1 ) 

+  ... 

sqrt (  sum(  res(:,2).A2  )  )  /  sensor { snr }. stddev (2 ) 

+  ... 

sqrt (  sum(  res(:,3).A2  )  )  /  sensor { snr }. stddev ( 3 ) 
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elseif  sensor { snr }. snr  type  ==  2  | |  sensor { snr }. snr  type  = 


%  azimuth  and  elevation  observations 

obs_ae  =  target { tgt }. obs (target { tgt }. obs_ff, 2 : 3) ; 

res  =  obs_ae (obsidx, : )  -  calc_rae ( : , 2 : 3 ) ; 

%  save  residuals 

stats { ls^iter, tgt }. res { snr }  =  res; 

%#ok<AGROW> 


%  plot  residuals 
if  koptions . echo 
if  snr  ==  1 
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r.'),  hold(resax2. 


cla(resax2),  plot(resax2,  obs_time,  res(:,l), 

'  on '  ) 

cla(resax3),  plot(resax3,  obs_time,  res(:,2), 
'r.'),  hold(resax3,  'on') 

drawnow 

else 

cols  =  ' rbmk ' ;  nc  =  length (cols) ; 
plot(resax2,  obs_time,  res(:,l),  [cols (mod (snr- 

l,nc)+l)  '.']) 

plot(resax3,  obs_time,  res(:,2),  [cols (mod (snr- 

1 , nc) +1 )  '.']) 

drawnow 

end 

end 

%  statistical  editting  of  outliers  based  on  sensor  std 

dev 

if  ls_iter  >  1 

res (  abs (res  ( : , 1 ) )  > 

sensor { snr }. stddev ( 1 ) *koptions . sig_edit,  1)  =  0; 

res (  abs ( res ( : , 2 ) )  > 

sensor { snr }. stddev (2 ) *koptions . sig_edit,  2)  =  0; 
end 

%  mark  editted  points 
if  koptions . echo 

editl  =  abs ( stats { ls_iter, tgt }. res { snr }(:, 1 ) )  > 

sensor{snr} . stddev ( 1 ) * koptions . sig_edit ; 

edit2  =  abs ( stats { ls_iter , tgt } . res { snr } ( : , 2 ) )  > 
sensor{snr} . stddev (2 ) * koptions . sig_edit ; 

if  any (editl),  plot(resax2,  obs_time (editl ) , 
stats { ls_iter, tgt }. res { snr } (editl , 1 ) ,  ' kx ' ) ,  end 

if  any(edit2),  plot(resax3,  obs_time (edit2 ) , 
stats { ls_iter , tgt } . res { snr } (edit2 , 2 ) ,  ' kx ' ) ,  end 

end 

%  cost  function  update 
cost_func  =  cost_func  +  ... 

sqrt (  sum(  res(:,l).A2  )  )  /  sensor { snr }. stddev ( 1 ) 

+  ... 

sqrt (  sum(  res(:,2).A2  )  )  /  sensor { snr }. stddev (2 ) ; 

%  remove  computed  range  values 
calc_rae ( : , 1 )  =  []; 
pert_rae ( : , 1 )  =  []; 

else 

error ([' Unrecognized  sensor  type  ID:  ' 
num2str (sensor { snr }. snr  type)]) 
end 

%  resize  residuals  matrix  to  a  vector 
res  =  reshape ( res ',[],!); 
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%  use  perturbed  trajectories  to  compute  observation 
matrices  (Ti) 

o, 

o 

%  each  observation  time  has  as  many  entries  as  is  has 
%  components  of  range,  azimuth,  or  elevation 
%  (Range,  Az,  &  El  -  3  entries  per  obs  time) 

%  (Az  &  El  -  2  entries  per  obs  time) 

o, 

o 

partials_x  =  calc_rae  -  pert_rae ( 1 : 6 : end, : ) ; 
partials_x  =  reshape (partials_x ' ,  [],  1); 

partials_y  =  calc_rae  -  pert_rae (2 : 6 : end, : ) ; 
partials_y  =  reshape (partials_y ' ,  [],  1); 

partials_z  =  calc_rae  -  pert_rae ( 3 : 6 : end, : ) ; 
partials_z  =  reshape (partials_z ' ,  [],  1); 

partials_vx  =  calc_rae  -  pert_rae ( 4 : 6 : end,  : )  ; 
partials  vx  =  reshape (partials  vx ' ,  [],  1); 

partials_vy  =  calc_rae  -  pert_rae ( 5 : 6 : end, : ) ; 
partials_vy  =  reshape (partials_vy ' ,  [],  1); 

partials_vz  =  calc_rae  -  pert_rae ( 6 : 6 : end, : ) ; 
partials_vz  =  reshape (partials_vz ' ,  [],  1); 

%  construct  the  observation  matrix:  dX(i)/dt 
T  =  [partials_x/sv_pert ( 1 )  partials_y/sv_pert (2 ) 
partials_z/sv_pert (3)  ... 

partials_vx/ sv_pert ( 4 )  partials_vy/ sv_pert ( 5 ) 
partials_vz/ sv_pert ( 6) ] ; 

%  construct  the  covariance  matrix 

Q  =  diag(  repmat ( 1 . /sensor { snr }. stddev . A2 ,  1,  numobs)  ) 
Qi  =  inv (Q) ; 

%  add  to  running  sums 
TtQiT  =  TtQiT  +  T ' *Qi*T; 

TtQir  =  TtQir  +  T'*Qi*res; 
end  %  end  of  loop  through  sensors 

%  compute  state  vector  update 
cov_update  =  pinv (TtQiT); 
sv_update  =  cov_update  *  TtQir; 

%  %  check  for  target  convergence  -  covariance  method 

%  cov_diag  =  reshape (cov_update, [], 1 ) ; 

%  cov_diag  =  cov_diag ( 1 : 7 : end) ; 

%  converged (tgt)  =  all ( sv_update  <=  sqrt (cov_diag) *le-7) ; 

%  check  for  target  convergence  -  cost  function  method 
if  Is  iter  >  1 
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converged (tgt)  =  abs (  ( stats { ls_iter-l , tgt }. cost_func  - 
cost_func)  /  stats { ls_iter-l , tgt }. cost_func  )  <  koptions . conv_tol ; 

%  if  cost  function  improved,  apply  correction 
if  cost_func  <  stats { ls_iter-l , tgt }. cost_func 

target { tgt }. init_sv  =  target { tgt }. init_sv  -  [sv_update' 

0  0  0]; 

end 

else 

%  first  run,  apply  correction 

target { tgt }. init_sv  =  target { tgt }. init_sv  -  [sv_update'  0  0 

0]; 

end 

if  koptions . echo  &&  converged (tgt) 

disp('  Convergence  Criteria  Met.') 

end 

%  save  statistics 

stats { ls_iter, tgt }. cost^func  =  cost_func; 

%#ok<AGROW> 

stats { ls_iter, tgt }. cov  =  cov_update; 

%#ok<AGROW> 

%  increment  iteration  number 
Is  iter  =  Is  iter  +  1; 
end  %  end  of  loop  through  targets 

end  %  end  of  iteration  while  loop 

%  fill  state  structure  with  states  during  free-flight 
for  tgt  =  l:numtgt 

%  time  series  to  propagate  to,  first  value  is  time  of  SV 
prop  time  =  [target { tgt }. init  time-time  pad; 
target { tgt } . obs_time (target { tgt } . obs_f f ) ] ; 

%  propagate  converged  state  vector 

[calc_time, calc_sv]  =  ode45 (@kaliper_eom,  prop_time, 
target { tgt }. init_sv ' ,  options); 

%  save  values 

state { tgt }. time  =  target { tgt }. obs_time; 

%#ok<AGROW> 

state { tgt }. sv  =  zeros ( length (target { tgt }. obs_time) , 9) ; 

%#ok<AGROW> 

state { tgt }. sv (target { tgt }. obs_ff, : )  =  calc_sv (2 : end, ; ) ; 

%#ok<AGROW> 

state { tgt }. cov  =  zeros ( length (target { tgt }. obs_time) , 81 ) ; 
%#ok<AGROW> 

full_cov  =  zeros(9,9); 

full_cov (1 : 6, 1 : 6)  =  stats { end, tgt }. cov; 
acc  cov  =  0.001; 
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full_cov(7, 7)  =  acc_cov;  full_cov ( 8, 8)  =  acc_cov;  full_cov (9, 9)  = 
acc_cov; 

state { tgt }. cov (target { tgt }. obs_ff, : )  =  repmat (  ... 
reshape ( full_cov, 1 ,  [ ] )  ,  ... 

length (find (target { tgt } . obs_f f ) )  ,  1 ) ; 

%#ok<AGROW> 

end 


on  oo 
o  o  o  o 

%%%%  Phase  2  state  vector  estimation  -  Reentry  -  Sensed  Accels  ~=  0 

oo  oo 
o  o  o  o 


if  koptions . echo 

disp('**  Entering  Phase  2  -  Non-Ballistic  Estimation  **') 

end 


0  0.00 
o  o  o  o 

%%%%  Phase  2a  -  least  squares  sliding  window  discontinuous  estimation 

0.0.  0.0. 
o  o  o  o 


%  check  whether  LSSW  should  be  performed 
if  koptions . lssw 

if  koptions . echo 

disp('  *  Phase  2a  -  LSSW  Beginning  *') 

end 

%  loop  through  targets 
for  tgt  =  l:numtgt 

%  this  will  only  function  on  3D  observations,  others  will  have 
to  be  interpolated 

all_time  =  target { tgt } . obs^time (-target {tgt} . obs_ff) ; 
all_eci  =  target { tgt }. obs_eci ( -target { tgt }. obs_ff, :) ; 
all  sv  =  zeros (length (all  time),  9); 

%  strip  out  NaNs  associated  with  non-3D  observations 
fit_idx  =  find (-isnan (all_eci ( : ,  1) ) ) ; 

%  data  to  fit  with  sliding  window 
fit  time  =  all  time (fit  idx)  ; 
fit_eci  =  all_eci (f it_idx, : ) ; 
numobs  =  length (fit  time); 

%  compute  lssw  window  size  for  each  observation 
win  pt  =  [1,  koptions . window  size/2,  numobs- 
koptions . window  size/2,  numobs]; 

win  win  =  [koptions .min  window  size,  koptions . window  size, 
koptions . window  size  koptions. max  window  size]; 

win  size  =  ceil ( interpl (win  pt,  win  win,  ltnumobs)); 

for  obs  =  1: numobs 
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win  max  =  min(numobs,  max(l+win  size, 
obs+ceil (win_size/2) ) ) ; 

win  min  =  win  max  -  win  size; 

win  time  =  fit  time (win  mintwin  max) ; 
win  eci  =  fit  eci (win  min:win  max, ; ) ; 
numfit  =  length (win  time); 

%  precompute  times 

dt  =  win  time  -  win  time(l); 

dt2  =  dt.A2; 

dt3  =  dt.A3; 

dt4  =  dt.A4; 

%  construct  A  matrix 
A  =  []; 

A(:,l)  =  ones (numf it, 1 ) ; 

A  ( : , 2 )  =  dt; 

A  (  : , 3 )  =  dt2/2; 

A  ( : , 4 )  =  dt3/6; 

A  (  :  , 5 )  =  dt4/24; 

%  separate  b  vectors  for  X,Y,Z  of  observations 
bx  =  win  eci(:,l); 
by  =  win_eci ( ; ,  2 )  ; 
bz  =  win_eci ( ; ,  3 )  ; 

%  least  squares  by  Singular  Value  Decomposition  for 

stability 

pinv  A  =  pinv (A) ;  %  this  is  much  faster  than  using  svd ( ) 
and  pinv()  seperately 

xh  x  =  pinv  A  *  bx; 

xh  y  =  pinv  A  *  by; 

xh  z  =  pinv  A  *  bz; 

all  sv(fit  idx(obs),:)  =  [xh  x(l)  xh  y(l)  xh  z(l)  xh  x(2) 
xh  y(2)  xh  z(2)  xh  x(3)  xh  y(3)  xh  z ( 3 ) ]  ; 

%  %  subtract  gravity  acceleration  from  computed 

accelerations 

%  grav  acc  =  calc  grav([xh  x(l)  xh  y(l)  xh  z(l)]); 

%  all_sv (fit_idx (obs) , 7 : 9)  =  all_sv (fit_idx (obs) , 7 : 9)  - 

grav_acc; 

end 

state { tgt }. lssw_std  =  std (all_sv (fit_idx, 1 : 3) -fit_eci ( : , 1 : 3) ) 

%  save  SV  solutions  to  use  as  initial  solutions  in  Phase  2b 
state { tgt }. sv ( -target { tgt }. obs_ff, 1 : 3 )  =  all_sv ( : , 1 : 3) ; 

end 

if  koptions . echo 
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disp('  *  Phase  2a  -  LSSW  Complete  *') 

end 

end 

oo  oo 
o  o  o  o 

%%%%  Phase  2b  -  Kalman  Filter  pass  with  weighted  LSSW  results 

oo.  on 
o  o  o  o 

if  koptions . echo 

disp('  *  Phase  2b  -  KF  Beginning  *') 

end 

%  loop  through  targets 
for  tgt  =  l:numtgt 

%  non-free  flight  indeces 

kf_idx  =  f ind ( -target { tgt }. obs_ff) ; 

for  i  =  1: length (kf  idx) 

%  index  of  this  observation  in  the  overall  array 
obsidx  =  kf  idx(i); 

obs^time  =  target { tgt }. obs_time (obsidx)  ; 
snridx  =  target { tgt }. obs_snr (obsidx) ; 

%  pull  out  previous  state  vector  for  easy  access 
prev_sv  =  state { tgt }. sv (obsidx-1 ,:) ; 

%  propagate  previous  state  to  this  time  for  the  prediction 
[prop_time, prop_sv]  =  ode45 (@kaliper_eom, 
target { tgt }. obs_time (obsidx-1 : obsidx)  ,  prev_sv ' ,  options); 

X  minus  =  prop  sv(end, :); 

%  compute  state  transition  matrix  to  propagate  covariance  from 
prev  state 

delta_t  =  diff (target { tgt }. obs_time (obsidx-1 : obsidx) ) ; 

F  =  calc_state_dist (state{tgt) . sv (obsidx-1,  :)) ; 

Phi  =  eye (9)  +  F  *  delta_t; 

%  compute  noise  on  covariance  propagation 
%  KADRE  Q  Method 
cov_noise  =  zeros (9); 

q  scale  =  diag ([ koptions . qp  koptions. qp  koptions. qp  ... 

koptions. qv  koptions. qv  koptions. qv  ... 
koptions. qa  koptions. qa  koptions . qa] ) ; 

for  j  =0:2 

for  k  =  0:2 

cov_noise  =  cov_noise  +  1/ (factorial (j )  *  factorial ( k) ) 

* 

FAj  *  q_scale  *  (F')Ak  *  delta_t A ( 1+ j +k) / ( 1+ j +k) ; 

end 

end 
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%  MSIC  Q  Method 

tot_acc  =  kaliper_calcaccel (prev_sv) ; 

acc_mag  =  sqrt (  tot_acc(l)A2  +  tot_acc(2)A2  +  tot_acc(3)A2  ); 

cov_pos  =  koptions.qp  *  tot_acc  *  delta_tA4  /  acc_mag; 

cov  vel  =  koptions.qv  *  tot  acc  *  delta  tA2  /  acc  mag; 

cov_acc  =  koptions.qa  *  tot_acc  /  acc_mag; 

cov_noise  =  diag ( [cov_pos  cov_vel  cov_acc] ) ; 

%  propagate  previous  covariance  as  the  prediction 
if  i  ==  1 

init_cov  =  diag ( [0.01  0.01  0.01  0.001  0.001  0.001  0.001 
0.001  0.001] ) ; 

P  minus  =  Phi  *  init  cov  *  Phi'  +  cov  noise; 

else 

P_minus  =  Phi  *  reshape ( state { tgt }. cov (obsidx-1 ,:), 9, 9)  * 

Phi'  +  cov  noise; 
end 


Q. 

O 

O, 

O 

O, 

O 

O, 

o 

Q. 

O 

Q. 

O 

o, 

o 


time 


o, 

o 

o, 

o 

o, 

o 

o, 

o 

o, 

o 

o, 

o 

o, 

o 

o, 

o 


compute  observation  matrix,  H 


(X-X0) /R 

(Y-Y0) /R 

(Z-Z0) /R 

0 

0 

0 

0 

0 

01 

dAz/ dX 

dAz/ dY 

dAz/ dZ 

0 

0 

0 

0 

0 

01 

dEl/dX 

dEl/dY 

dEl/dZ 

0 

0 

0 

0 

0 

01 

R  =  [  (X-X0 ) A2  +  ( Y-Y0 ) A2  +  (Z-Z0)A2  ] A  ( 1/2 ) 

(X0,Y0,Z0)  -  Inertial  location  of  observer  @  observation 


%  determine  position  of  observing  sensor 

if  sensor { snridx }. snr  type  ==  1  | |  sensor { snridx }. snr  type  ==  2 

%  stationary  sensor 

sen  eci  =  tm  ecr2eci ( sensor { snridx }. pos  ecf,  0,  obs  time, 

0); 

sen_ecf  =  sensor { snridx }. pos_ecf; 

sen  11a  =  tm  ecr211a (sensor { snridx} .pos  ecf); 

sen_lla(l)  =  geoc2geod ( sen_lla ( 1 ) ) ; 

sen  tm  =  sensor { snridx }. tm; 

elseif  sensor { snridx }. snr  type  ==  3  | |  sensor { snridx }. snr  type 

==  4 

%  moving  sensor,  interpolate  sensor  position  at  obs  time 
interp  lat  =  interpl ( sensor { snr }. pos  time, 
sensor { snr }. pos_lla (:, 1 ) ,  obs_time,  'spline'); 

interp  Ion  =  interpl ( sensor { snr }. pos  time, 
sensor { snr }. pos_lla (;, 2 ) ,  obs_time,  'spline'); 

interp  alt  =  interpl ( sensor { snr }. pos  time, 
sensor { snr }. pos_lla (:, 3 ) ,  obs_time,  'spline'); 


sen  11a  =  [interp  lat,  interp  Ion,  interp  alt]; 

[sen  ecf,  sen  tm]  =  calc  sensor ( interp  lat,  interp  Ion, 

interp_alt) ; 

sen  eci  =  tm  ecr2eci(sen  ecf,  0,  obs  time,  0) ; 
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end 


%  compute  range 

rng  =  sqrt (  (X  minus (1) -sen  eci(l))A2  +  (X  minus(2)- 
sen_eci (2 ) ) A2  +  (X_minus ( 3 ) -sen_eci ( 3 ) ) A2  ); 

%  convert  X  minus  to  the  observation  variables 
sim  ecf  =  tm  eci2ecr(X  minus (1:3),  0,  obs  time,  0) ; 
sim  sez  =  tm  ecr2sez(sim  ecf,  sen  ecf,  sen  tm) ; 
sim  rae  =  tm  sez2rae(sim  sez)  ; 

%  determine  whether  we  need  the  range  component 
if  target { tgt }. obs (obsidx, 1 )  ~=  0 

H  =  zeros (3,9)  ; 

%  fill  in  range  partials 

H(l,l:3)  =  (X_minus ( 1 : 3 ) -sen_eci )  /  rng; 

%  row  indeces  to  compute  Az  and  El  partials  in 
a  z  i  =  2  ; 
e  1  i  =  3  ; 

%  define  simulated  &  actual  observations  -  full  RAE 
sim  z  =  sim  rae; 

obs_z  =  target { tgt }. obs (obsidx, :) ; 

else 

H  =  zeros (2,9) ; 

%  row  indeces  to  compute  Az  and  El  partials  in 
a  z  i  =  1  ; 
e  1  i  =  2  ; 

%  define  simulated  &  actual  observations  -  only  AE 

sim  z  =  sim  rae (2:3); 

obs_z  =  target { tgt }. obs (obsidx, 2 : 3 ) ; 

end 

%  fill  in  azimuth  partials  (see  appendix) 

%  compute  common  terms  contained  in  partials: 

%  sin  &  cos  of  sensor  position 
slat  =  sin ( sen_lla ( 1 ) ) ; 
clat  =  cos (sen_lla (1) ) ; 
slon  =  sin (sen_lla (2) ) ; 
cion  =  cos (sen_lla (2) ) ; 

%  sin  &  cos  of  ECI->ECEF  angle 
st  =  sin (earth .AngVel  *  obs  time); 
ct  =  cos (earth .AngVel  *  obs  time); 

%  predicted  SV  position  components 
x  =  X  minus ( 1 ) ; 
y  =  X  minus (2 ) ; 
z  =  X  minus ( 3 )  ; 

%  azimuth  partials 
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numer  Az  =  -slon* (ct*x+st*y-sen  ecf ( 1 ) ) +clon* ( -st*x+ct*y- 
sen_ecf (2 ) ) ; 

denom  Az  =  -slat*clon* (ct*x+st*y-sen  ecf (1) ) -slat*slon* (- 
st*x+ct*y-sen_ecf (2) ) +clat* (z-sen_ecf (3) ) ; 

dAz  base  =  1/(1  +  (numer  Az/denom  Az)A2);  %  derivative  of  inv 

tan 

%  dAz/dX,  dY,  dZ 

H(azi,l)  =  dAz  base  *  ( (-slon*ct-clon*st) *denom  Az  -  (- 

slat*clon*ct+slat*slon*st) *numer  Az)  /  denom  AzA2; 

H(azi,2)  =  dAz  base  *  ( (-slon*st+clon*ct) *denom  Az  -  (- 

slat*clon*st-slat*slon*ct) *numer  Az)  /  denom  AzA2; 

H(azi,3)  =  dAz  base  *  (-clat*numer  Az)  /  denom  AzA2; 

%  elevation  partials 

numer  El  =  clat*clon* (ct*x+st*y-sen  ecf (1) ) +clat*slon* (- 
st*x+ct*y-sen_ecf (2) ) +slat* (z-sen_ecf (3) ) ; 
denom  El  =  rng; 

dEl  base  =  l/sqrt(l  -  (numer  El/denom  E1)A2);  %  derivative  of 

inv  sin 

%  dEl/ dX,  dY,  dZ 

H(eli,l)  =  dEl  base  *  ( (clat*clon*ct  -  clat*slon*st) *denom  El 

(x-sen  eci ( 1 )) /denom  El*numer  El)  /  denom  E1A2; 

H(eli,2)  =  dEl  base  *  ( (clat*clon*st  +  clat*slon*ct) *denom  El 

(y-sen  eci (2 )) /denom  El*numer  El)  /  denom  E1A2; 

H(eli,3)  =  dEl  base  *  (slat*denom  El  -  (z- 
sen  eci ( 3 )) /denom  El*numer  El)  /  denom  E1A2; 

%  if  a  solution  from  the  LSSW  pass  was  computed,  add  it  to  the 
calculation 

if  koptions . lssw  &&  -all (state{tgt) . sv (obsidx, 1 : 3)  ==  0) 

%  add  SV  components  to  observation  &  simulation  variables 
obs_z  =  [obs_z  state{tgt} . sv (obsidx,  1 : 3)  ]  ; 
sim  z  =  [sim  z  X  minus (1:3)]; 

%  add  derivatives  to  H  matrix 

%  H  =  [H;  zeros  (6, 3),  eye (6, 6);  zeros (3, 9)];  %  (complete 

SV) 

H  =  [H;  eye (3, 3),  zeros (3, 6)];  %  (position  SV) 

%  construct  measurement  noise  matrix  w/  SV  components 
R  =  diag ([ sensor { snridx }. stddev . A2 , 
state { tgt } . lssw_std . A2 ] ) ; 
else 

%  construct  measurement  noise  matrix 
R  =  diag ( sensor { snridx }. stddev . A2 ) ; 

end 

%  with  H&R  computed,  calculate  the  Kalman  Gain 
K  =  P  minus*H'  *  pinv(H*P  minus*H'  +  R) ; 

%  compute  SV  update 

X  plus  =  X  minus (: )  +  K* (obs  z(:)  -  sim  z(:)); 
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%  compute  covariance  update 
P  plus  =  (eye (9)  -  K*H) *P  minus; 

%  save  results  from  this  data  point 

state { tgt }. sv  minus (obsidx, : )  =  X  minus'; 

state { tgt }. cov_minus (obsidx, ; )  =  P_minus ( : ) ' ; 

state { tgt }. sv (obsidx, ; )  =  X_plus ' ; 

state { tgt }. cov (obsidx, : )  =  P_plus ( : ) ' ; 

if  koptions . echo 

disp ([' Completed  KF  Pass  -  Filter  Time  = 
num2str(obs  time)]) 
end 

end 

end 

if  koptions . echo 

disp('  *  Phase  2b  -  KF  Complete  *') 

end 

if  koptions . echo 

disp('  *  Phase  2c  -  Smoother  Beginning  *') 

end 


0,000 
o  o  o  o 

%%%%  Phase  2c  -  Smoother 

0,0  0,0 
o  o  o  o 


%  loop  through  targets 
for  tgt  =  l:numtgt 

%  non-free  flight  indeces 
kf_idx  =  f ind ( -target { tgt }. obs_ff) ; 
state { tgt }. cov_smooth  =  state { tgt }. cov; 
state { tgt }. sv_smooth  =  state { tgt }. sv; 

%  run  smoother  backwards 
for  i  =  length (kf  idx)-l:-l:l 

%  index  of  this  observation  in  the  overall  array 
obsidx  =  kf  idx(i); 

obs^time  =  target { tgt }. obs_time (obsidx)  ; 

%  compute  state  transition  matrix  to  propagate  covariance  from 
prev  state 

delta_t  =  diff (target { tgt }. obs_time (obsidx : obsidx+1 )) ; 

F  =  calc_state_dist (state{tgt} . sv (obsidx, :)) ; 

Phi  =  eye (9)  +  F  *  delta_t; 

C  =  reshape ( state { tgt }. cov (obsidx, :), 9, 9)  *  Phi'  *  ... 

pinv ( reshape ( state { tgt } . cov_minus (obsidx+1 , :) ,9,9) ) ; 
P_smooth  =  reshape ( state { tgt }. cov (obsidx, ;), 9, 9)  +  C  *  ... 
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( reshape ( state { tgt }. cov_smooth (obsidx+1 9, 9)  -  ... 
reshape ( state { tgt }. cov_minus (obsidx+1 9,  9)  )  *  C'; 

state { tgt }. cov_smooth (obsidx, : )  =  P_smooth ( : ) ' ; 
state { tgt }. sv_smooth (obsidx, : )  =  state { tgt }. sv (obsidx, : )  +  ... 
(C  *  (state{tgt} . sv_smooth (obsidx+1, :) '  - 
state { tgt } . sv_minus (obsidx+1 , : ) ' ) ) ' ; 

if  koptions . echo 

disp ([' Completed  KF  Pass  -  Smoother  Time  =  ' 
num2str(obs  time)]) 
end 

end 

end 

if  koptions . echo 

disp('  *  Phase  2c  -  Smoother  Complete  *') 

end 


oo  oo 
o  o  o  o 

%%%%  Phase  3  -  Wrap  up.  Parameter  Computation 

oo  on 
o  o  o  o 


if  koptions . echo 

disp('**  Phase  3  -  Parameter  Computation  **') 

end 

%  loop  through  targets 
for  tgt  =  lrnumtgt 

%%%%  compute  residuals 

sv_ecf  =  tm_eci2ecr ( state { tgt } . sv_smooth ( : , 1 : 3 ) ,  0, 
state { tgt }. time,  0)  ; 

sv_sez  =  zeros (size (sv_ecf) ) ; 

%  number  of  sensors  for  this  target 
tgtsnr  =  unique (target { tgt }. obs_snr) ; 
numsnr  =  length (tgtsnr) ; 

%  loop  through  sensors,  computing  SEZ  information 
for  snridx  =  1: numsnr 

snr  =  tgtsnr ( snridx)  ; 

%  observations  from  this  sensor,  and  those  observation  times 
obsidx  =  target { tgt }. obs_snr  ==  snr; 
obs_time  =  target { tgt }. obs^time (obsidx) ; 

%  stationary  range,  az,  el  OR  az,  el  observations 
if  sensor { snr }. snr  type  ==  1  | |  sensor { snr }. snr  type  ==  2 

%  convert  ECEF  SV  positions  to  stationary  SEZ 
sv_sez (obsidx, : )  =  tm_ecr2sez (sv_ecf (obsidx, :) , 
sensor{snr} ,pos_ecf,  sensorfsnr} .tm) ; 
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%  moving  range,  az,  el  OR  az,  el  observations 
elseif  sensor { snr }. snr  type  ==  3  | |  sensor { snr }. snr  type  == 

%  interpolate  sensor  positions  at  observation  times 
interp  lat  =  interpl ( sensor { snr }. pos  time, 
sensor { snr }. pos_lla (:, 1 ) ,  obs_time,  'spline'); 

interp  Ion  =  interpl ( sensor { snr }. pos  time, 
sensor { snr }. pos_lla (:, 2 ) ,  obs_time,  'spline'); 

interp  alt  =  interpl ( sensor { snr }. pos  time, 
sensor { snr }. pos_lla (:, 3 ) ,  obs_time,  'spline'); 

[sensor  pos,  sensor  tm]  =  calc  sensor  move (interp  lat, 
interp  Ion,  interp  alt) ; 

%  convert  ECEF  SV  positions  to  moving  SEZ 
sv  sez (obsidx, : )  =  tm  ecr2sez  move ( sv  ecf (obsidx, : ) , 
sensor_pos,  sensor_tm) ; 
else 

error ([' Unrecognized  sensor  type  ID:  ' 
num2str (sensor { snr }. snr  type)]) 
end 

end 

%  compute  RAE  state 
sv  rae  =  tm  sez2rae (sv  sez); 

target { tgt }. res  =  repmat (NaN,  size (sv_rae) ) ; 

%  loop  through  sensors,  computing  residuals 
for  snridx  =  ltnumsnr 

snr  =  tgtsnr ( snridx) ; 

%  observations  from  this  sensor 
obsidx  =  target { tgt }. obs_snr  ==  snr; 

if  sensor { snr }. snr  type  ==  1  | |  sensor { snr }. snr  type  ==  3 

%  range,  azimuth,  and  elevation  observations 
target { tgt }. res (obsidx, : )  =  target { tgt }. obs (obsidx, : )  - 

sv_rae (obsidx,  : )  ; 

elseif  sensor { snr }. snr  type  ==  2  | |  sensor { snr }. snr  type  ==  4 

%  azimuth  and  elevation  observations 

target { tgt }. res (obsidx, 2 : 3 )  =  target { tgt }. obs (obsidx, 2 : 3 )  - 
sv_rae (obsidx, 2:3) ; 
end 

end 

%  plot  residuals,  if  requested 
if  koptions . echo 

figure,  hold  on 

%  loop  through  sensors,  computing  residuals 
for  snridx  =  l:numsnr 

snr  =  tgtsnr (snridx); 
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%  observations  from  this  sensor 
obsidx  =  target { tgt }. obs_snr  ==  snr; 
if  snr  ==  1 

subplot  (3, 1, 1) ,  hold  on 
plot (target { tgt } . obs_time (obsidx)  , 
target { tgt }. res (obsidx, 1 ) ,  ' r . ' ) 

subplot  (3, 1, 2) ,  hold  on 
plot (target { tgt } . obs_time (obsidx)  , 
target { tgt }. res (obsidx, 2 )  ,  'r.') 

subplot  ( 3 , 1 , 3 ) ,  hold  on 
plot (target { tgt } . obs_time (obsidx) , 
target { tgt }. res (obsidx, 3 ) ,  'r.') 

else 

cols  =  ' rbmk ' ;  nc  =  length (cols) ; 
subplot  (3,1,1) 

plot (target { tgt } . obs_time (obsidx)  , 
target { tgt }. res (obsidx, 1 ) ,  [cols (mod ( snr-1 , nc) +1 )  '.']) 

subplot  (3, 1,2) 

plot (target { tgt } . obs_time (obsidx) , 
target { tgt }. res (obsidx, 2 ) ,  [cols (mod ( snr-1 , nc) +1 )  '.']) 

subplot (3,1,3) 

plot (target { tgt } . obs_time (obsidx) , 
target { tgt }. res (obsidx, 3 ) ,  [cols (mod (snr-1, nc) +1)  '.']) 

end 

end 

end 


%%%%  compute  lat/lon/alt  positions 

state { tgt }. 11a  =  tm_eci2vel ( state { tgt } . sv_smooth ( : , 1 : 3 ) , 
state {tgt} .time) ; 

state { tgt } . 11a ( : , 3 )  =  state { tgt } . 11a ( : , 3) /1000; 

%%%%  compute  altitude 

state { tgt }. alt  =  state { tgt } . 11a ( : , 3) ; 

%%%%  compute  ballistic  coefficient 
%  atmospheric  values 

[temp,  pres,  dens]  =  calc_atmos ( state { tgt }. alt ) ; 
%#ok<NASGU> 

%  air  relative  velocity  for  dynamic  pressure 
vxa  =  state { tgt } . sv_smooth ( : , 4 )  + 
earth . AngVel . * state { tgt } . sv_smooth ( : , 2 ) ; 

vya  =  state { tgt } . sv_smooth ( : ,  5 )  - 
earth .AngVel . * state { tgt } . sv_smooth ( : , 1) ; 
vz  =  state { tgt } . sv_smooth ( : , 6) ; 
va  =  sqrt (  vxa.A2  +  vya.A2  +  vz.A2  ); 

%  dynamic  pressure  (convert  dens  from  kg/m3  to  kg/km3) 
dynpres  =  .5  *  1000 A3  *  dens  .  *  va.A2; 

%  magnitude  of  drag  acceleration 
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drag_mag  =  sqrt (  state { tgt } . sv_smooth ( : , 7 ) . A2  + 
state { tgt } . sv_smooth ( : , 8 ) . A2  +  state { tgt } . sv_smooth ( : , 9) . A2  ); 

%  ballistic  coefficient  in  kg/m2 

state { tgt }. beta  =  dynpres  ./  drag_mag  /  1000A2; 

%%%%  Mach  number  (non-dim) 
gamma  =  1.4; 
gas_const  =  284; 

state { tgt } .mach  =  va  .  /  sqrt (  gamma  *  gas  const  *  temp  )  *  1000 

end 

if  koptions . echo 

disp('**  KALIPER  Run  Complete  **') 

end 

%  set  output  variables 
if  nargout  ==  1 

%  one  output  requested 
varargout{l}  =  state; 
elseif  nargout  ==  2 

%  two  outputs  requested 
varargout{l}  =  state; 
varargout{2}  =  target; 
elseif  nargout  ==  3 

%  three  outputs  requested 
varargout{l}  =  state; 
varargout{2}  =  target; 
varargout{3}  =  stats; 

end 


%KALIPER_CALCINIT  -  compute  initial  guess  for  state 

g, 

o 

%  Initial  guess  based  on  least  squares  solution  to  equations 
motion 


%  assuming  constant  4th  derivative  of  position: 
%  x  measured  = 


o, 

o 

g. 

o 

g, 

o 

g, 

o 

g, 

o 

g, 

o 

g. 

o 

g, 

o 


x  true  + 
x_true ( 1 ) 
x_true (2 ) 
x_true ( 3 ) 
x  true ( 4 ) 


*  T  + 

*  TA2  /  2  + 

*  TA3  /  6  + 

*  TA4  /  24  +  noise 


Note:  number  in  ()  is  the  nth  derivative 


of 


function  [init  time,  init  sv]  =  kaliper  calcinit (tgt) 
numobs  =  length (target { tgt }. obs_time) ; 

%  check  if  free  flight  observations  have  been  predetermined 
if  isfield (target { tgt } ,  ' obs_ff') 

target { tgt }. obs_ff  =  false (numobs , 1 ) ; 

end 
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the 


if  -isfield (target { tgt } ,  ' obs_ff')  I  I  -any (target { tgt }. obs_ff) 

%  initial  attempt  will  be  to  select  observations  outside  of 

%  Eath's  atmosphere 

obs_alt  =  calc_alt (target { tgt }. obs_eci ) ; 
num  3d  =  length (find (-isnan (obs  alt))); 

target { tgt }. obs  ff  =  obs  alt  >=  earth .AtmAlt; 

%  check  to  see  if  there  is  still  nothing  defined 
if  -any (target { tgt }. obs_ff) 

%  select  highest  altitude  points  until  10%  of  available 

are 

%  selected  (arbitrary) 
alt_step  =  -1; 

cutoff_alt  =  max(obs_alt)  +  alt_step; 
while  length ( find (target { tgt }. obs  ff))/num  3d  <  .1 
target { tgt }. obs_ff  =  obs_alt  >=  cutoff_alt; 
cutoff_alt  =  cutoff_alt  +  alt_step; 

end 

end 

end 

%  compute  least  squares  solution 

fit_time  =  target { tgt }. obs^time (target { tgt }. obs_ff) ; 
fit_eci  =  target { tgt }. obs_eci (target { tgt }. obs_ff, ;) ; 
numfit  =  length (fit  time); 

%  precompute  times 


dt  = 

:  fit 

time  -  fit  ' 

dt2 

=  dt 

•  A2; 

dt3 

=  dt 

.  A3; 

dt4 

=  dt 

.  A4; 

%  construct  A  matrix 

A  = 

[]  ; 

A(:, 

l)  = 

ones (numfit 

A(:, 

2)  = 

dt; 

A  (  :  , 

3)  = 

dt2/2; 

A  ( :  , 

4)  = 

dt3/6; 

A  ( ; , 

5)  = 

dt4/24; 

%  separate  b  vectors  for  X,Y,Z  of  observations 
bx  =  f it_eci ( : , 1) ; 
by  =  f it_eci ( : ,  2)  ; 
bz  =  f it_eci ( : ,  3)  ; 

%  least  squares  by  Singular  Value  Decomposition  for  stability 
pinv  A  =  pinv (A) ;  %  this  is  much  faster  than  using  svd ( )  and 
pinv()  seperately 

xh  x  =  pinv  A  *  bx; 

xh  y  =  pinv  A  *  by; 

xh  z  =  pinv  A  *  bz; 
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%  compile  initial  guess 
init  time  =  fit  time(l); 

init  sv  =  [xh  x(l)  xh  y(l)  xh  z(l)  xh  x(2)  xh  y(2)  xh  z(2) 
xh_x ( 3 )  xh_y ( 3 )  xh_z ( 3 ) ] ; 

%  for  initialization  purposes,  we're  assuming  free-flight 
init_sv (7:9)  =  0; 

%  use  the  time  determined  by  the  3-D  data  to  include  others 

ff_time  =  target { tgt }. obs_time (target { tgt }. obs_ff) ; 

target { tgt }. obs_ff  =  target { tgt }. obs_time  >=  ff_time(l)  &  ... 

target { tgt }. obs_time  <=  ff_time (end) ; 

end  %  end  of  kaliper  calcinit  function 

%KALIPER  EOM  -  Generatlized  equations  of  motion 

Q. 

O 

%  These  equations  of  motion  can  be  used  with  ode45  and  the  state 
to 

%  propagate  the  state  forward  in  time. 

o, 

o 

%  Note:  This  is  a  modification  to  METAL's  eom  re  and  eom  ff. 

Since 

%  the  sensed  acceleration  is  an  element  in  the  state  vector,  these 
%  EOMs  satisfy  both  free  flight  and  reentry  conditions 

o, 

o 

Q. 

O 

function  dy  =  kaliper_eom (t, y) 

%#ok<INUSL> 


%  change  y  from  single  vector  to  columns  of  vectors 

nval  =  size (y, 1 ) ; 

nvec  =  nval/9; 

y  =  reshape (y,  9,  nvec); 

dy  =  zeros (9,  nvec) ; 

%  compute  gravitational  acceleration 
grav_acc  =  calc_grav (y (1 : 3,  : )  '  )  '  ; 

%  compute  total  acceleration 
tot_acc  =  grav_acc  +  y(7:9,:); 


%  derivatives 


dy  ( 1 ,  : 

=  y  ( 4 ,  : )  ; 

%  dx/dt 

=  vx 

dy (2 ,  : 

=  y  ( 5 ,  : )  ; 

%  dy/dt 

=  vy 

dy (3,  : 

=  y  (6,  : )  ; 

%  dz/dt 

=  vz 

dy  ( 4 ,  : 

=  tot  acc ( 1 , : 

) ;  %  d2x/dt2 

=  tax 

dy (5,  : 

=  tot  acc (2 , : 

) ;  %  d2y/ dt2 

=  tay 

dy  ( 6 ,  : 

=  tot  acc (3, : 

) ;  %  d2z/dt2 

=  taz 

%  acceleration  derivatives  =  0 

%  change  dy  back  to  single  column  to  satisfy  ode45 
dy  =  reshape (dy,  nval,  1); 
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end  %  end  of  kaliper  eom  function 

%CALC  STATE  DIST  -  calculate  the  state  distribution  matrix  (F)  for 
a  given  SV. 

%  used  to  compute  state  transition  matrix  by:  Phi 

=  I  +  F*dt 

function  f  =  calc_state_dist ( sv) 

%  compute  partials  of  the  state,  initialize  with  all  zeros 
f  =  zeros (9,9) ; 

%  position  derivates 
f  (1,4)  =  1;  f  (2,5)  =  1;  f (3, 6)  =  1; 

%  terms  for  velocity  partials 
r  =  sqrt(sv(l)A2  +  sv(2)A2  +  sv(3)A2); 
mu  =  earth . GravConst; 

%  x  velocity  gravity  partials  (simple  gravity) 
f  (4 , 1 )  =  -mu/rA3  *  (1  -  3*sv (1) A2/rA2) ; 
f  (4,2)  =  3*mu*sv (1) *sv(2) /rA5; 
f  (4,3)  =  f (4,2) /sv (2 ) *sv (3) ; 

%  y  velocity  gravity  partials 
f (5,1)  =  f (4,2) ; 

f  (5,2)  =  -mu/rA3  *  (1  -  3*sv (2) A2/rA2) ; 
f  (5,3)  =  f (5,1) /sv ( 1 ) *sv (3) ; 

%  z  velocity  gravity  partials 
f (6,1)  =  f (4,3) ; 
f (6,2)  =  f (5,3)  ; 

f  (6, 3)  =  -mu/rA3  *  (1  -  3*sv (3) A2/rA2) ; 

%  velocity  sensed  accel  partials 
f  (4,7)  =  1;  f  ( 5 , 8 )  =  1;  f  (6,  9)  =  1; 

end  %  end  of  calc  state  dist  function 

end  %  end  of  kaliper  main  function 


102 


read_data.m 


read_data  .  m  is  a  MATLAB  script  written  to  read  in  data  from  truth  cases.  Original 
formatting  is  preserved  to  maintain  functionality  when  pasted  into  MATLAB. 


function  [target, sensor]  =  read_data (datafile,  varargin) 

snr  =  1; 
tgt  =  1 ; 

if  nargin  ==  1 

target { tgt }. obs  =  []; 
target { tgt }. obs_time  =  []; 
target { tgt }. obs_snr  =  []; 
elseif  nargin  ==  3 

target  =  varargin{l}; 
sensor  =  varargin{2}; 

tgt  =  length (target)  +  1; 
target { tgt }. obs  =  []; 
target { tgt }. obs_time  =  []; 
target { tgt }. obs_snr  =  []; 

for  i  =  1 : length (target) 

snr  =  max (max (target { i }. obs  snr),  snr); 

end 

snr  =  snr  +  1; 
elseif  nargin  ==  4 

target  =  varargin{l}; 
sensor  =  varargin{2}; 
snr  =  varargin{3}; 

tgt  =  length (target)  +  1; 
target { tgt }. obs  =  []; 
target { tgt }. obs_time  =  []; 
target { tgt }. obs_snr  =  []; 
elseif  nargin  ==  5 

target  =  varargin{l}; 
sensor  =  varargin{2}; 
snr  =  varargin{3}; 
tgt  =  varargin{4}; 

if  -isfield (target { tgt } ,  'obs'),  target { tgt }. obs  =  [];  end 

if  -isfield (target { tgt } ,  'obs_time'),  target { tgt }. obs_time  =  []; 

end 

if  -isfield (target { tgt } ,  'obs_snr'),  target { tgt }. obs_snr  =  [];  end 

end 

fid  =  fopen (datafile); 
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sen_pos  =  fgetl(fid); 
dataline  =  fgetl(fid); 
fclose (fid) ; 

[sen_lat,  sen_lon,  sen_alt]  =  strread ( sen_pos ,  ' %n  %n  %n '  )  ; 

numdata  =  length (strread (dataline,  ' %n ' ) )  -  1; 

if  numdata  ==  3 
%  RAE 

[obs_time,  obs_r,  obs_a,  obs_e]  =  textread (datafile,  ' %n  %n  %n 
'headerlines',  1); 

target { tgt }. obs  =  [ target { tgt }. obs ;  [obs_r(:),  obs_a ( : ) *pi/180, 

obs_e ( : ) *pi/180] ] ; 

sensor { snr }. snr_type  =  1; 

sensor { snr }. stddev  =  [.002  .03  *  pi/180,  .03  *  pi/180]; 
sensor { snr }. obs_bias  =[0  0  0]  ; 
elseif  numdata  ==  2 
%  AE 

[obs_time,  obs_a,  obs_e]  =  textread (datafile,  ' %n  %n  %n ' , 
'headerlines',  1); 

target { tgt }. obs  =  [ target { tgt }. obs ;  [zeros (size (obs_a (;)) ) 
obs_a ( ; ) *pi/180,  obs_e ( : ) *pi/180] ] ; 

sensor { snr }. snr_type  =2; 

sensor { snr }. stddev  =  [.03  *  pi/180,  .03  *  pi/180]; 

sensor { snr }. obs_bias  =  [0  0]  ; 

else 

error ([' Unable  to  identify  data  file  with  '  num2str (numdata)  ' 
entries  per  line.']) 
end 

target { tgt }. obs_time  =  [target { tgt }. obs_time;  obs_time] ; 
target { tgt }. obs_snr  =  [target { tgt }. obs_snr;  repmat (snr, 
size (obs_time) ) ] ; 

[target { tgt }. obs_time,  idx]  =  sort(target{tgt}.obs_time); 
target  { tgt }.  obs  =  target  { tgt }.  obs  ( idx,  re¬ 
target  { tgt }.  obs_snr  =  target{tgt}.obs_snr(idx); 

target { tgt }. numobs  =  length(target{tgt}.obs_time); 

sensor { snr }. pos_lla  =  [sen_lat*pi/180,  sen_lon*pi/180,  sen_alt/1000 
[ sensor { snr }. pos_ecf,  sensor { snr }. tm]  =  calc_sensor (sen_lat*pi/180, 
sen_lon*pi/180,  sen_alt/1000 ) ; 
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run_kaliper_cases.m 

run_kaliper_cases  .  m  is  a  MATLAB  script  written  to  analyze  truth  cases  and 
demonstrate  input  syntax.  Original  formatting  is  preserved  to  maintain  functionality 
when  pasted  into  MATLAB. 


close  all 
clear 

%%%  Items  for  computing  truth  accelerations 
define  air 

read_stdatmos ( ' stdatmos7  6 . dat ' ) 

%  RV  definition 
rv_beta  =  5600; 
rv^hca  =  9.5; 
rv_br  =  0.24; 

define  berman(rv  hca,  rv  br) 

0,0,0, 
o  o  o 

%%%%%%  Scenario  1  -  Pure  Ballistic 

%%%  Case  1  -  Single  Sensor  -  lx  Data  Rate,  along  heading 
%  files  =  { ' scenl_senl_posl_ratel . rae ' } ; 

%%%  Case  2  -  Single  Sensor  -  lx  Data  Rate,  perp  to  heading 
%  files  =  { ' scenl  senl  pos2  ratel.rae'}; 

%%%  Case  3  -  Single  Sensor  -  lx  Data  Rate,  45  deg  to  heading 
files  =  { ' scenl_sen2_pos3_ratel . rae ' } ; 

%%%  Case  4  -  Single  Sensor  -  2x  Data  Rate,  best  position 
%  files  =  {'scenl  senl  posl  rate2.rae'}; 

%%%  Case  5-2  Sensors  -  lx  Data  Rate,  along  &  along  positions 
%  files  =  {'scenl  senl  posl  ratel.rae',  'scenl  sen2  posl  ratel.rae'}; 
%%%  Case  6-2  Sensors  -  lx  Data  Rate,  along  &  45  deg  to  positions 
%  files  =  {'scenl  senl  posl  ratel.rae',  'scenl  sen2  pos2  ratel.rae'}; 
%%%  Case  7-2  Sensors  -  lx  Data  Rate,  along  &  perp  to  positions 
%  files  =  {'scenl  senl  posl  ratel.rae',  'scenl  sen2  pos3  ratel.rae'}; 
%%%  Case  8-2  Sensors  -  lx&2x  Data  Rate,  along  &  perp  to  positions 
%  files  =  {'scenl  senl  posl  ratel.rae',  'scenl  sen2  posl  rate2.rae'}; 
%%%  Case  9-2  Sensors  -  2x&lx  Data  Rate,  along  &  perp  to  positions 
%  files  =  {'scenl  senl  posl  rate2.rae',  'scenl  sen2  pos3  ratel.rae'}; 
%%%  Case  10-2  Sensors  -  2x  Data  Rate,  along  &  perp  to  positions 
%  files  =  {'scenl  senl  posl  rate2.rae',  'scenl  sen2  pos3  rate2.rae'}; 
%%%  Case  11-2  Sensors  -  2x  Data  Rate,  perp  to  &  perp  to  positions 
%  files  =  {'scenl  senl  pos3  rate2.rae',  'scenl  sen2  pos3  rate2.rae'}; 
%%%  Case  12-3  Sensors  -  lx  Data  Rate,  along,  along,  &  along  positions 
%  files  =  {'scenl  senl  posl  ratel.rae',  'scenl  sen2  posl  ratel.rae', 

' scenl_sen3_posl_ratel . rae ' } ; 

%%%  Case  13-3  Sensors  -  lx  Data  Rate,  along,  along,  &  45  deg  to 
positions 
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%  files  =  { ' scenl  senl  posl  ratel.rae',  ' scenl  sen2  posl  ratel.rae', 

' scenl_sen3_pos2_ratel . rae ' } ; 

%%%  Case  14-3  Sensors  -  lx  Data  Rate,  along,  along,  &  perp  to 
positions 

%  files  =  {'scenl  senl  posl  ratel.rae',  'scenl  sen2  posl  ratel.rae', 

' scenl_sen3_pos3_ratel . rae ' } ; 

%%%  Case  15-3  Sensors  -  lx  Data  Rate,  along,  along,  &  perp  to 
positions 

%  files  =  {'scenl  senl  posl  ratel.rae',  'scenl  sen2  posl  ratel.rae', 

' scenl_sen3_pos2_rate2 . rae ' } ; 

%%%  Case  16-2  Sensors  (RAE  &  AE)  -  lx  Data  Rate,  along  &  along 
positions 

%  files  =  {'scenl  senl  posl  ratel.rae',  'scenl  sen3  posl  ratel.rae'}; 
%%%  Case  17-2  Sensors  (RAE  &  AE)  -  lx  Data  Rate,  along  &  45  deg  to 
positions 

%  files  =  {'scenl  senl  posl  ratel.rae',  'scenl  sen3  pos2  ratel.rae'}; 
%%%  Case  18-2  Sensors  (RAE  &  AE)  -  lx  Data  Rate,  along  &  perp  to 
positions 

%  files  =  { ' scenl_senl_posl_ratel . rae ' ,  ' scenl_sen3_pos3_ratel . rae ' } ; 

%%%  Case  19-2  Sensors  (RAE  &  AE)  -  lx&2x  Data  Rate,  along  &  45  deg  to 
positions 

%  files  =  {'scenl  senl  posl  ratel.rae',  'scenl  sen3  pos2  rate2.rae'}; 
%%%%%%  Scenario  2  -  Single  Maneuver 

%%%  Case  20  -  '  Sensors  (RAE)  -  lx  Data  Rate,  along  position 
%  files  =  { ' scen2  senl  posl  ratel.rae'}; 

%%%  Case  21  -  '  Sensors  (RAE)  -  lx  Data  Rate,  45  deg  to  position 
%  files  =  { ' scen2  senl  pos2  ratel.rae'}; 

%%%  Case  22  -  '  Sensors  (RAE)  -  lx  Data  Rate,  perp  to  position 
%  files  =  { ' scen2_senl_pos3_ratel . rae ' } ; 

%%%  Case  23  -  Single  Sensor  -  2x  Data  Rate,  best  position 
%  files  =  { ' scen2_senl_pos3_rate2 . rae ' } ; 

%%%  Case  24-2  Sensors  -  lx  Data  Rate,  along  &  along  positions 
%  files  =  { ' scen2  senl  posl  ratel.rae',  ' scen2  sen2  posl  ratel.rae'}; 
%%%  Case  25-2  Sensors  -  lx  Data  Rate,  along  &  45  deg  to  positions 
%  files  =  { ' scen2  senl  posl  ratel.rae',  ' scen2  sen2  pos2  ratel.rae'}; 
%%%  Case  26-2  Sensors  -  lx  Data  Rate,  along  &  perp  to  positions 
%  files  =  { ' scen2  senl  posl  ratel.rae',  ' scen2  sen2  pos3  ratel.rae'}; 
%%%  Case  27-2  Sensors  -  lx  Data  Rate,  45  deg  to  &  along  positions 
%  files  =  { ' scen2  senl  pos2  ratel.rae',  ' scen2  sen2  posl  ratel.rae'}; 
%%%  Case  28-2  Sensors  -  lx  Data  Rate,  45  deg  to  &  45  deg  to  positions 
%  files  =  { ' scen2  senl  pos2  ratel.rae',  ' scen2  sen2  pos2  ratel.rae'}; 
%%%  Case  29-2  Sensors  -  lx  Data  Rate,  45  deg  to  &  perp  to  positions 
%  files  =  { ' scen2  senl  pos2  ratel.rae',  ' scen2  sen2  pos3  ratel.rae'}; 
%%%  Case  30-2  Sensors  -  lx&2x  Data  Rate,  45  deg  to  &  along  positions 
%  files  =  { ' scen2  senl  pos2  ratel.rae',  ' scen2  sen2  posl  rate2.rae'}; 
%%%  Case  31-3  Sensors  -  lx  Data  Rate,  along,  45  deg  to,  &  along 
positions 

%  files  =  { ' scen2  senl  posl  ratel.rae',  ' scen2  sen2  pos2  ratel.rae', 

' scen2_sen3_posl_ratel . ae ' } ; 

%%%  Case  32-3  Sensors  -  lx  Data  Rate,  along,  45  deg  to,  &  45  deg  to 
positions 

%  files  =  { ' scen2  senl  posl  ratel.rae',  ' scen2  sen2  pos2  ratel.rae', 

' scen2_sen3_pos2_ratel . ae ' } ; 
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%%%  Case  33-3  Sensors  -  lx  Data  Rate,  along,  45  deg  to,  &  perp  to 
positions 

%  files  =  { ' scen2  senl  posl  ratel.rae',  ' scen2  sen2  pos2  ratel.rae', 

' scen2_sen3_pos3_ratel . ae ' } ; 

%%%  Case  34-3  Sensors  -  lx,2x,2x  Data  Rate,  along,  45  deg  to,  &  perp 
to  positions 

%  files  =  { ' scen2  senl  posl  ratel.rae',  ' scen2  sen2  pos2  rate2.rae', 

' scen2_sen3_pos3_rate2 . ae ' } ; 

%%%  Case  35-2  Sensors  (RAE  &  AE)  -  lx  Data  Rate,  along  &  along 
positions 

%  files  =  { ' scen2  senl  posl  ratel.rae',  ' scen2  sen3  posl  ratel.ae'}; 

%%%  Case  36-2  Sensors  (RAE  &  AE)  -  lx  Data  Rate,  along  &  45  deg  to 
positions 

%  files  =  { ' scen2  senl  posl  ratel.rae',  ' scen2  sen3  pos2  ratel.ae'}; 

%%%  Case  37-2  Sensors  (RAE  &  AE)  -  lx  Data  Rate,  along  &  perp  to 
positions 

%  files  =  { ' scen2  senl  posl  ratel.rae',  ' scen2  sen3  pos3  ratel.ae'}; 

%%%  Case  38-2  Sensors  (RAE  &  AE)  -  lx&2x  Data  Rate,  along  &  45  deg  to 
positions 

%  files  =  { ' scen2  senl  posl  ratel.rae',  ' scen2  sen3  pos2  rate2.ae'}; 
%%%%%%  Scenario  3  -  Double  Maneuver 

%%%  Case  39  -  '  Sensors  (RAE)  -  lx  Data  Rate,  along  position 
%  files  =  { ' scen3_senl_posl_ratel . rae ' } ; 

%%%  Case  40  -  '  Sensors  (RAE)  -  lx  Data  Rate,  45  deg  to  position 
%  files  =  { ' scen3_senl_pos2_ratel . rae ' } ; 

%%%  Case  41  -  '  Sensors  (RAE)  -  lx  Data  Rate,  perp  to  position 
%  files  =  { ' scen3_senl_pos3_ratel . rae ' } ; 

%%%  Case  42  -  Single  Sensor  -  2x  Data  Rate,  best  position 
%  files  =  { ' scen3_senl_pos2_rate2 . rae ' } ; 

%%%  Case  43-2  Sensors  -  lx  Data  Rate,  along  &  along  positions 
%  files  =  { ' scen3  senl  posl  ratel.rae',  ' scen3  sen2  posl  ratel.rae'}; 
%%%  Case  44-2  Sensors  -  lx  Data  Rate,  along  &  45  deg  to  positions 
%  files  =  { ' scen3  senl  posl  ratel.rae',  ' scen3  sen2  pos2  ratel.rae'}; 
%%%  Case  45-2  Sensors  -  lx  Data  Rate,  along  &  perp  to  positions 
%  files  =  { ' scen3_senl_posl_ratel . rae ' ,  ' scen3_sen2_pos3_ratel . rae ' } ; 

%%%  Case  46-2  Sensors  -  lx  Data  Rate,  45  deg  to  &  along  positions 
%  files  =  { ' scen3  senl  pos2  ratel.rae',  ' scen3  sen2  posl  ratel.rae'}; 
%%%  Case  47-2  Sensors  -  lx  Data  Rate,  45  deg  to  &  45  deg  to  positions 
%  files  =  { ' scen3  senl  pos2  ratel.rae',  ' scen3  sen2  pos2  ratel.rae'}; 
%%%  Case  48-2  Sensors  -  lx  Data  Rate,  45  deg  to  &  perp  to  positions 
%  files  =  { ' scen3_senl_pos2_ratel . rae ' ,  ' scen3_sen2_pos3_ratel . rae ' } ; 

%%%  Case  49-2  Sensors  -  lx&2x  Data  Rate,  45  deg  to  &  along  positions 
%  files  =  { ' scen3  senl  pos2  ratel.rae',  ' scen3  sen2  posl  rate2.rae'}; 
%%%  Case  50-3  Sensors  -  lx  Data  Rate,  along,  45  deg  to,  &  along 
positions 

%  files  =  { ' scen3  senl  posl  ratel.rae',  ' scen3  sen2  pos2  ratel.rae', 

' scen3_sen3_posl_ratel . ae ' } ; 

%%%  Case  51-3  Sensors  -  lx  Data  Rate,  along,  45  deg  to,  &  45  deg  to 
positions 

%  files  =  { ' scen3  senl  posl  ratel.rae',  ' scen3  sen2  pos2  ratel.rae', 

' scen3_sen3_pos2_ratel . ae ' } ; 

%%%  Case  52-3  Sensors  -  lx  Data  Rate,  along,  45  deg  to,  &  perp  to 
positions 
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%  files  =  { ' scen3  senl  posl  ratel.rae',  ' scen3  sen2  pos2  ratel.rae', 

' scen3_sen3_pos3_ratel . ae ' } ; 

%%%  Case  53-3  Sensors  -  lx,2x,2x  Data  Rate,  along,  45  deg  to,  &  45 
deg  to  positions 

%  files  =  { ' scen3  senl  posl  ratel.rae',  ' scen3  sen2  pos2  rate2.rae', 

' scen3_sen3_pos2_rate2 . ae ' } ; 

%%%  Case  54-2  Sensors  (RAE  &  AE)  -  lx  Data  Rate,  along  &  along 
positions 

%  files  =  { ' scen3_senl_posl_ratel . rae ' ,  ' scen3_sen3_posl_ratel . ae  '  }  ; 

%%%  Case  55-2  Sensors  (RAE  &  AE)  -  lx  Data  Rate,  along  &  45  deg  to 
positions 

%  files  =  { ' scen3_senl_posl_ratel . rae ' ,  ' scen3_sen3_pos2_ratel . ae ' } ; 

%%%  Case  56-2  Sensors  (RAE  &  AE)  -  lx  Data  Rate,  along  &  perp  to 
positions 

%  files  =  { ' scen3_senl_posl_ratel . rae ' ,  ' scen3_sen3_pos3_ratel . ae ' } ; 

%%%  Case  57-2  Sensors  (RAE  &  AE)  -  lx&2x  Data  Rate,  along  &  45  deg  to 
positions 

%  files  =  { ' scen3_senl_posl_ratel . rae ' ,  ' scen3_sen3_pos2_rate2 . ae ' } ; 

%  indeces  for  truth  comparisons 

seen  =  1; 

rate  =  [1,1,1]; 

%  read  in  data  files 

[target, sensor]  =  read_data ( files { 1 }) ; 
for  i  =  2 : length ( files ) 

[target, sensor]  =  read_data ( files { i }, target , sensor , i , 1 ) ; 

end 

%  run  kaliper 
%  koptions . echo  =  true(l); 
koptions.max  iter  =  15; 

%  koptions . conv_tol  =  .00001; 
koptions . lssw  =  false  (1); 
koptions . sig_edit  =  20; 
koptions. qp  =  0.0; 
koptions. qv  =  0.0; 
koptions. qa  =  0.001; 

tic,  [state,  target]  =  kaliper (target,  sensor,  koptions);  toe 

load  all_case_truth 

eci  =  [ ] ; 
acc  =  [ ] ; 
comp  =  [  ] ; 
ts  =  []  ; 

for  i  =  1 : length ( files ) 

traj_acc  =  calc_accel (traj_eci { seen, rate ( i )} ,  rv_beta*10A6,  0,  0) ; 
traj_acc  =  traj_acc  -  calc_grav (traj_eci { seen, rate ( i ) } ) ; 
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(40  -  alt (alt 


if  seen  ==  2 

alt  =  calc_alt (traj_eci { seen,  rate (i)  } )  ; 
traj_acc(alt  <=  40,1)  =  traj_acc(alt  <=  40,1)  - 
<=  40) ) /20*0.0098; 

elseif  seen  ==  3 

alt  =  calc_alt (tra j_eci { seen,  rate ( i )})  ; 
traj_acc(alt  <=  40,1)  =  traj_acc(alt  <=  40,1)  -  (40  -  alt (alt 
<=  40) ) /20*0.0098; 

traj_acc(alt  <=  20,2)  =  traj_acc(alt  <=  20,2)  -  (20  -  alt (alt 
<=  20) ) /5*0.0098; 
end 

eci  =  [eci;  tra j_eci { seen,  rate ( i )}]  ; 

ace  =  [ace;  traj_acc] ; 

ts  =  [ts;  tra j_time { seen, rate ( i )}]  ; 

snridx  =  target { 1 }. obs  snr  ==  i; 

comp  =  [comp;  state { 1 }. sv_smooth ( snridx,  :)]  ; 

end 

[ts,id]  =  sort(ts); 
eci  =  eci ( id,  ; )  ; 
acc  =  acc ( id, : )  ; 
comp  =  comp (id, ; ) ; 

stateerr  =  comp  -  [eci,  acc] ; 

staterms  =  sqrt (  mean (  (stateerr) . A2  )  )  *  1000; 

%  easy  to  copy  from: 
fprintf ( ' % . If \n ' ,  staterms) 
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